mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 16:23:46 +08:00
Mariah
This commit is contained in:
firestar5683
Binary file not shown.
@@ -407,6 +407,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"NavDesiresAllowed", {PERSISTENT, BOOL, "1", "0", 2}},
|
||||
{"NavLongitudinalAllowed", {PERSISTENT, BOOL, "1", "0", 2}},
|
||||
{"ClearNavOnOffroad", {PERSISTENT, BOOL, "1", "1", 2}},
|
||||
{"ClearNavOnOffroadTimeoutMinutes", {PERSISTENT, INT, "0", "0", 2}},
|
||||
{"NavDestination", {PERSISTENT | CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"NavInstructionCollapsed", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0", "0"}},
|
||||
{"NavInstructionState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}},
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-1c2627e1-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-2a5cb66b-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-1c2627e1-DEBUG
|
||||
DEV-2a5cb66b-DEBUG
|
||||
@@ -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_287802830867790833) {
|
||||
out_287802830867790833[0] = delta_x[0] + nom_x[0];
|
||||
out_287802830867790833[1] = delta_x[1] + nom_x[1];
|
||||
out_287802830867790833[2] = delta_x[2] + nom_x[2];
|
||||
out_287802830867790833[3] = delta_x[3] + nom_x[3];
|
||||
out_287802830867790833[4] = delta_x[4] + nom_x[4];
|
||||
out_287802830867790833[5] = delta_x[5] + nom_x[5];
|
||||
out_287802830867790833[6] = delta_x[6] + nom_x[6];
|
||||
out_287802830867790833[7] = delta_x[7] + nom_x[7];
|
||||
out_287802830867790833[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_2092251631667009235) {
|
||||
out_2092251631667009235[0] = delta_x[0] + nom_x[0];
|
||||
out_2092251631667009235[1] = delta_x[1] + nom_x[1];
|
||||
out_2092251631667009235[2] = delta_x[2] + nom_x[2];
|
||||
out_2092251631667009235[3] = delta_x[3] + nom_x[3];
|
||||
out_2092251631667009235[4] = delta_x[4] + nom_x[4];
|
||||
out_2092251631667009235[5] = delta_x[5] + nom_x[5];
|
||||
out_2092251631667009235[6] = delta_x[6] + nom_x[6];
|
||||
out_2092251631667009235[7] = delta_x[7] + nom_x[7];
|
||||
out_2092251631667009235[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_1374850324761980275) {
|
||||
out_1374850324761980275[0] = -nom_x[0] + true_x[0];
|
||||
out_1374850324761980275[1] = -nom_x[1] + true_x[1];
|
||||
out_1374850324761980275[2] = -nom_x[2] + true_x[2];
|
||||
out_1374850324761980275[3] = -nom_x[3] + true_x[3];
|
||||
out_1374850324761980275[4] = -nom_x[4] + true_x[4];
|
||||
out_1374850324761980275[5] = -nom_x[5] + true_x[5];
|
||||
out_1374850324761980275[6] = -nom_x[6] + true_x[6];
|
||||
out_1374850324761980275[7] = -nom_x[7] + true_x[7];
|
||||
out_1374850324761980275[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_2300837471234553583) {
|
||||
out_2300837471234553583[0] = -nom_x[0] + true_x[0];
|
||||
out_2300837471234553583[1] = -nom_x[1] + true_x[1];
|
||||
out_2300837471234553583[2] = -nom_x[2] + true_x[2];
|
||||
out_2300837471234553583[3] = -nom_x[3] + true_x[3];
|
||||
out_2300837471234553583[4] = -nom_x[4] + true_x[4];
|
||||
out_2300837471234553583[5] = -nom_x[5] + true_x[5];
|
||||
out_2300837471234553583[6] = -nom_x[6] + true_x[6];
|
||||
out_2300837471234553583[7] = -nom_x[7] + true_x[7];
|
||||
out_2300837471234553583[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_6646352482882938111) {
|
||||
out_6646352482882938111[0] = 1.0;
|
||||
out_6646352482882938111[1] = 0.0;
|
||||
out_6646352482882938111[2] = 0.0;
|
||||
out_6646352482882938111[3] = 0.0;
|
||||
out_6646352482882938111[4] = 0.0;
|
||||
out_6646352482882938111[5] = 0.0;
|
||||
out_6646352482882938111[6] = 0.0;
|
||||
out_6646352482882938111[7] = 0.0;
|
||||
out_6646352482882938111[8] = 0.0;
|
||||
out_6646352482882938111[9] = 0.0;
|
||||
out_6646352482882938111[10] = 1.0;
|
||||
out_6646352482882938111[11] = 0.0;
|
||||
out_6646352482882938111[12] = 0.0;
|
||||
out_6646352482882938111[13] = 0.0;
|
||||
out_6646352482882938111[14] = 0.0;
|
||||
out_6646352482882938111[15] = 0.0;
|
||||
out_6646352482882938111[16] = 0.0;
|
||||
out_6646352482882938111[17] = 0.0;
|
||||
out_6646352482882938111[18] = 0.0;
|
||||
out_6646352482882938111[19] = 0.0;
|
||||
out_6646352482882938111[20] = 1.0;
|
||||
out_6646352482882938111[21] = 0.0;
|
||||
out_6646352482882938111[22] = 0.0;
|
||||
out_6646352482882938111[23] = 0.0;
|
||||
out_6646352482882938111[24] = 0.0;
|
||||
out_6646352482882938111[25] = 0.0;
|
||||
out_6646352482882938111[26] = 0.0;
|
||||
out_6646352482882938111[27] = 0.0;
|
||||
out_6646352482882938111[28] = 0.0;
|
||||
out_6646352482882938111[29] = 0.0;
|
||||
out_6646352482882938111[30] = 1.0;
|
||||
out_6646352482882938111[31] = 0.0;
|
||||
out_6646352482882938111[32] = 0.0;
|
||||
out_6646352482882938111[33] = 0.0;
|
||||
out_6646352482882938111[34] = 0.0;
|
||||
out_6646352482882938111[35] = 0.0;
|
||||
out_6646352482882938111[36] = 0.0;
|
||||
out_6646352482882938111[37] = 0.0;
|
||||
out_6646352482882938111[38] = 0.0;
|
||||
out_6646352482882938111[39] = 0.0;
|
||||
out_6646352482882938111[40] = 1.0;
|
||||
out_6646352482882938111[41] = 0.0;
|
||||
out_6646352482882938111[42] = 0.0;
|
||||
out_6646352482882938111[43] = 0.0;
|
||||
out_6646352482882938111[44] = 0.0;
|
||||
out_6646352482882938111[45] = 0.0;
|
||||
out_6646352482882938111[46] = 0.0;
|
||||
out_6646352482882938111[47] = 0.0;
|
||||
out_6646352482882938111[48] = 0.0;
|
||||
out_6646352482882938111[49] = 0.0;
|
||||
out_6646352482882938111[50] = 1.0;
|
||||
out_6646352482882938111[51] = 0.0;
|
||||
out_6646352482882938111[52] = 0.0;
|
||||
out_6646352482882938111[53] = 0.0;
|
||||
out_6646352482882938111[54] = 0.0;
|
||||
out_6646352482882938111[55] = 0.0;
|
||||
out_6646352482882938111[56] = 0.0;
|
||||
out_6646352482882938111[57] = 0.0;
|
||||
out_6646352482882938111[58] = 0.0;
|
||||
out_6646352482882938111[59] = 0.0;
|
||||
out_6646352482882938111[60] = 1.0;
|
||||
out_6646352482882938111[61] = 0.0;
|
||||
out_6646352482882938111[62] = 0.0;
|
||||
out_6646352482882938111[63] = 0.0;
|
||||
out_6646352482882938111[64] = 0.0;
|
||||
out_6646352482882938111[65] = 0.0;
|
||||
out_6646352482882938111[66] = 0.0;
|
||||
out_6646352482882938111[67] = 0.0;
|
||||
out_6646352482882938111[68] = 0.0;
|
||||
out_6646352482882938111[69] = 0.0;
|
||||
out_6646352482882938111[70] = 1.0;
|
||||
out_6646352482882938111[71] = 0.0;
|
||||
out_6646352482882938111[72] = 0.0;
|
||||
out_6646352482882938111[73] = 0.0;
|
||||
out_6646352482882938111[74] = 0.0;
|
||||
out_6646352482882938111[75] = 0.0;
|
||||
out_6646352482882938111[76] = 0.0;
|
||||
out_6646352482882938111[77] = 0.0;
|
||||
out_6646352482882938111[78] = 0.0;
|
||||
out_6646352482882938111[79] = 0.0;
|
||||
out_6646352482882938111[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_4027971756970364591) {
|
||||
out_4027971756970364591[0] = 1.0;
|
||||
out_4027971756970364591[1] = 0.0;
|
||||
out_4027971756970364591[2] = 0.0;
|
||||
out_4027971756970364591[3] = 0.0;
|
||||
out_4027971756970364591[4] = 0.0;
|
||||
out_4027971756970364591[5] = 0.0;
|
||||
out_4027971756970364591[6] = 0.0;
|
||||
out_4027971756970364591[7] = 0.0;
|
||||
out_4027971756970364591[8] = 0.0;
|
||||
out_4027971756970364591[9] = 0.0;
|
||||
out_4027971756970364591[10] = 1.0;
|
||||
out_4027971756970364591[11] = 0.0;
|
||||
out_4027971756970364591[12] = 0.0;
|
||||
out_4027971756970364591[13] = 0.0;
|
||||
out_4027971756970364591[14] = 0.0;
|
||||
out_4027971756970364591[15] = 0.0;
|
||||
out_4027971756970364591[16] = 0.0;
|
||||
out_4027971756970364591[17] = 0.0;
|
||||
out_4027971756970364591[18] = 0.0;
|
||||
out_4027971756970364591[19] = 0.0;
|
||||
out_4027971756970364591[20] = 1.0;
|
||||
out_4027971756970364591[21] = 0.0;
|
||||
out_4027971756970364591[22] = 0.0;
|
||||
out_4027971756970364591[23] = 0.0;
|
||||
out_4027971756970364591[24] = 0.0;
|
||||
out_4027971756970364591[25] = 0.0;
|
||||
out_4027971756970364591[26] = 0.0;
|
||||
out_4027971756970364591[27] = 0.0;
|
||||
out_4027971756970364591[28] = 0.0;
|
||||
out_4027971756970364591[29] = 0.0;
|
||||
out_4027971756970364591[30] = 1.0;
|
||||
out_4027971756970364591[31] = 0.0;
|
||||
out_4027971756970364591[32] = 0.0;
|
||||
out_4027971756970364591[33] = 0.0;
|
||||
out_4027971756970364591[34] = 0.0;
|
||||
out_4027971756970364591[35] = 0.0;
|
||||
out_4027971756970364591[36] = 0.0;
|
||||
out_4027971756970364591[37] = 0.0;
|
||||
out_4027971756970364591[38] = 0.0;
|
||||
out_4027971756970364591[39] = 0.0;
|
||||
out_4027971756970364591[40] = 1.0;
|
||||
out_4027971756970364591[41] = 0.0;
|
||||
out_4027971756970364591[42] = 0.0;
|
||||
out_4027971756970364591[43] = 0.0;
|
||||
out_4027971756970364591[44] = 0.0;
|
||||
out_4027971756970364591[45] = 0.0;
|
||||
out_4027971756970364591[46] = 0.0;
|
||||
out_4027971756970364591[47] = 0.0;
|
||||
out_4027971756970364591[48] = 0.0;
|
||||
out_4027971756970364591[49] = 0.0;
|
||||
out_4027971756970364591[50] = 1.0;
|
||||
out_4027971756970364591[51] = 0.0;
|
||||
out_4027971756970364591[52] = 0.0;
|
||||
out_4027971756970364591[53] = 0.0;
|
||||
out_4027971756970364591[54] = 0.0;
|
||||
out_4027971756970364591[55] = 0.0;
|
||||
out_4027971756970364591[56] = 0.0;
|
||||
out_4027971756970364591[57] = 0.0;
|
||||
out_4027971756970364591[58] = 0.0;
|
||||
out_4027971756970364591[59] = 0.0;
|
||||
out_4027971756970364591[60] = 1.0;
|
||||
out_4027971756970364591[61] = 0.0;
|
||||
out_4027971756970364591[62] = 0.0;
|
||||
out_4027971756970364591[63] = 0.0;
|
||||
out_4027971756970364591[64] = 0.0;
|
||||
out_4027971756970364591[65] = 0.0;
|
||||
out_4027971756970364591[66] = 0.0;
|
||||
out_4027971756970364591[67] = 0.0;
|
||||
out_4027971756970364591[68] = 0.0;
|
||||
out_4027971756970364591[69] = 0.0;
|
||||
out_4027971756970364591[70] = 1.0;
|
||||
out_4027971756970364591[71] = 0.0;
|
||||
out_4027971756970364591[72] = 0.0;
|
||||
out_4027971756970364591[73] = 0.0;
|
||||
out_4027971756970364591[74] = 0.0;
|
||||
out_4027971756970364591[75] = 0.0;
|
||||
out_4027971756970364591[76] = 0.0;
|
||||
out_4027971756970364591[77] = 0.0;
|
||||
out_4027971756970364591[78] = 0.0;
|
||||
out_4027971756970364591[79] = 0.0;
|
||||
out_4027971756970364591[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_2311726086880249539) {
|
||||
out_2311726086880249539[0] = state[0];
|
||||
out_2311726086880249539[1] = state[1];
|
||||
out_2311726086880249539[2] = state[2];
|
||||
out_2311726086880249539[3] = state[3];
|
||||
out_2311726086880249539[4] = state[4];
|
||||
out_2311726086880249539[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_2311726086880249539[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_2311726086880249539[7] = state[7];
|
||||
out_2311726086880249539[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_6249326857146976875) {
|
||||
out_6249326857146976875[0] = state[0];
|
||||
out_6249326857146976875[1] = state[1];
|
||||
out_6249326857146976875[2] = state[2];
|
||||
out_6249326857146976875[3] = state[3];
|
||||
out_6249326857146976875[4] = state[4];
|
||||
out_6249326857146976875[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_6249326857146976875[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_6249326857146976875[7] = state[7];
|
||||
out_6249326857146976875[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_1431608970589135594) {
|
||||
out_1431608970589135594[0] = 1;
|
||||
out_1431608970589135594[1] = 0;
|
||||
out_1431608970589135594[2] = 0;
|
||||
out_1431608970589135594[3] = 0;
|
||||
out_1431608970589135594[4] = 0;
|
||||
out_1431608970589135594[5] = 0;
|
||||
out_1431608970589135594[6] = 0;
|
||||
out_1431608970589135594[7] = 0;
|
||||
out_1431608970589135594[8] = 0;
|
||||
out_1431608970589135594[9] = 0;
|
||||
out_1431608970589135594[10] = 1;
|
||||
out_1431608970589135594[11] = 0;
|
||||
out_1431608970589135594[12] = 0;
|
||||
out_1431608970589135594[13] = 0;
|
||||
out_1431608970589135594[14] = 0;
|
||||
out_1431608970589135594[15] = 0;
|
||||
out_1431608970589135594[16] = 0;
|
||||
out_1431608970589135594[17] = 0;
|
||||
out_1431608970589135594[18] = 0;
|
||||
out_1431608970589135594[19] = 0;
|
||||
out_1431608970589135594[20] = 1;
|
||||
out_1431608970589135594[21] = 0;
|
||||
out_1431608970589135594[22] = 0;
|
||||
out_1431608970589135594[23] = 0;
|
||||
out_1431608970589135594[24] = 0;
|
||||
out_1431608970589135594[25] = 0;
|
||||
out_1431608970589135594[26] = 0;
|
||||
out_1431608970589135594[27] = 0;
|
||||
out_1431608970589135594[28] = 0;
|
||||
out_1431608970589135594[29] = 0;
|
||||
out_1431608970589135594[30] = 1;
|
||||
out_1431608970589135594[31] = 0;
|
||||
out_1431608970589135594[32] = 0;
|
||||
out_1431608970589135594[33] = 0;
|
||||
out_1431608970589135594[34] = 0;
|
||||
out_1431608970589135594[35] = 0;
|
||||
out_1431608970589135594[36] = 0;
|
||||
out_1431608970589135594[37] = 0;
|
||||
out_1431608970589135594[38] = 0;
|
||||
out_1431608970589135594[39] = 0;
|
||||
out_1431608970589135594[40] = 1;
|
||||
out_1431608970589135594[41] = 0;
|
||||
out_1431608970589135594[42] = 0;
|
||||
out_1431608970589135594[43] = 0;
|
||||
out_1431608970589135594[44] = 0;
|
||||
out_1431608970589135594[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_1431608970589135594[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1431608970589135594[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1431608970589135594[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1431608970589135594[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_1431608970589135594[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1431608970589135594[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1431608970589135594[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1431608970589135594[53] = -9.8100000000000005*dt;
|
||||
out_1431608970589135594[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_1431608970589135594[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1431608970589135594[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1431608970589135594[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1431608970589135594[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_1431608970589135594[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1431608970589135594[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_1431608970589135594[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1431608970589135594[62] = 0;
|
||||
out_1431608970589135594[63] = 0;
|
||||
out_1431608970589135594[64] = 0;
|
||||
out_1431608970589135594[65] = 0;
|
||||
out_1431608970589135594[66] = 0;
|
||||
out_1431608970589135594[67] = 0;
|
||||
out_1431608970589135594[68] = 0;
|
||||
out_1431608970589135594[69] = 0;
|
||||
out_1431608970589135594[70] = 1;
|
||||
out_1431608970589135594[71] = 0;
|
||||
out_1431608970589135594[72] = 0;
|
||||
out_1431608970589135594[73] = 0;
|
||||
out_1431608970589135594[74] = 0;
|
||||
out_1431608970589135594[75] = 0;
|
||||
out_1431608970589135594[76] = 0;
|
||||
out_1431608970589135594[77] = 0;
|
||||
out_1431608970589135594[78] = 0;
|
||||
out_1431608970589135594[79] = 0;
|
||||
out_1431608970589135594[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_1875491097223459739) {
|
||||
out_1875491097223459739[0] = 1;
|
||||
out_1875491097223459739[1] = 0;
|
||||
out_1875491097223459739[2] = 0;
|
||||
out_1875491097223459739[3] = 0;
|
||||
out_1875491097223459739[4] = 0;
|
||||
out_1875491097223459739[5] = 0;
|
||||
out_1875491097223459739[6] = 0;
|
||||
out_1875491097223459739[7] = 0;
|
||||
out_1875491097223459739[8] = 0;
|
||||
out_1875491097223459739[9] = 0;
|
||||
out_1875491097223459739[10] = 1;
|
||||
out_1875491097223459739[11] = 0;
|
||||
out_1875491097223459739[12] = 0;
|
||||
out_1875491097223459739[13] = 0;
|
||||
out_1875491097223459739[14] = 0;
|
||||
out_1875491097223459739[15] = 0;
|
||||
out_1875491097223459739[16] = 0;
|
||||
out_1875491097223459739[17] = 0;
|
||||
out_1875491097223459739[18] = 0;
|
||||
out_1875491097223459739[19] = 0;
|
||||
out_1875491097223459739[20] = 1;
|
||||
out_1875491097223459739[21] = 0;
|
||||
out_1875491097223459739[22] = 0;
|
||||
out_1875491097223459739[23] = 0;
|
||||
out_1875491097223459739[24] = 0;
|
||||
out_1875491097223459739[25] = 0;
|
||||
out_1875491097223459739[26] = 0;
|
||||
out_1875491097223459739[27] = 0;
|
||||
out_1875491097223459739[28] = 0;
|
||||
out_1875491097223459739[29] = 0;
|
||||
out_1875491097223459739[30] = 1;
|
||||
out_1875491097223459739[31] = 0;
|
||||
out_1875491097223459739[32] = 0;
|
||||
out_1875491097223459739[33] = 0;
|
||||
out_1875491097223459739[34] = 0;
|
||||
out_1875491097223459739[35] = 0;
|
||||
out_1875491097223459739[36] = 0;
|
||||
out_1875491097223459739[37] = 0;
|
||||
out_1875491097223459739[38] = 0;
|
||||
out_1875491097223459739[39] = 0;
|
||||
out_1875491097223459739[40] = 1;
|
||||
out_1875491097223459739[41] = 0;
|
||||
out_1875491097223459739[42] = 0;
|
||||
out_1875491097223459739[43] = 0;
|
||||
out_1875491097223459739[44] = 0;
|
||||
out_1875491097223459739[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_1875491097223459739[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1875491097223459739[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1875491097223459739[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1875491097223459739[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_1875491097223459739[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1875491097223459739[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1875491097223459739[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1875491097223459739[53] = -9.8100000000000005*dt;
|
||||
out_1875491097223459739[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_1875491097223459739[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1875491097223459739[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1875491097223459739[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1875491097223459739[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_1875491097223459739[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1875491097223459739[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_1875491097223459739[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1875491097223459739[62] = 0;
|
||||
out_1875491097223459739[63] = 0;
|
||||
out_1875491097223459739[64] = 0;
|
||||
out_1875491097223459739[65] = 0;
|
||||
out_1875491097223459739[66] = 0;
|
||||
out_1875491097223459739[67] = 0;
|
||||
out_1875491097223459739[68] = 0;
|
||||
out_1875491097223459739[69] = 0;
|
||||
out_1875491097223459739[70] = 1;
|
||||
out_1875491097223459739[71] = 0;
|
||||
out_1875491097223459739[72] = 0;
|
||||
out_1875491097223459739[73] = 0;
|
||||
out_1875491097223459739[74] = 0;
|
||||
out_1875491097223459739[75] = 0;
|
||||
out_1875491097223459739[76] = 0;
|
||||
out_1875491097223459739[77] = 0;
|
||||
out_1875491097223459739[78] = 0;
|
||||
out_1875491097223459739[79] = 0;
|
||||
out_1875491097223459739[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_4297991490361286945) {
|
||||
out_4297991490361286945[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_5658454479721392301) {
|
||||
out_5658454479721392301[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_7005560747898744396) {
|
||||
out_7005560747898744396[0] = 0;
|
||||
out_7005560747898744396[1] = 0;
|
||||
out_7005560747898744396[2] = 0;
|
||||
out_7005560747898744396[3] = 0;
|
||||
out_7005560747898744396[4] = 0;
|
||||
out_7005560747898744396[5] = 0;
|
||||
out_7005560747898744396[6] = 1;
|
||||
out_7005560747898744396[7] = 0;
|
||||
out_7005560747898744396[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_8931663582155029431) {
|
||||
out_8931663582155029431[0] = 0;
|
||||
out_8931663582155029431[1] = 0;
|
||||
out_8931663582155029431[2] = 0;
|
||||
out_8931663582155029431[3] = 0;
|
||||
out_8931663582155029431[4] = 0;
|
||||
out_8931663582155029431[5] = 0;
|
||||
out_8931663582155029431[6] = 1;
|
||||
out_8931663582155029431[7] = 0;
|
||||
out_8931663582155029431[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_806466490562007731) {
|
||||
out_806466490562007731[0] = state[4];
|
||||
out_806466490562007731[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8388151047956984123) {
|
||||
out_8388151047956984123[0] = state[4];
|
||||
out_8388151047956984123[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_4832911148893244830) {
|
||||
out_4832911148893244830[0] = 0;
|
||||
out_4832911148893244830[1] = 0;
|
||||
out_4832911148893244830[2] = 0;
|
||||
out_4832911148893244830[3] = 0;
|
||||
out_4832911148893244830[4] = 1;
|
||||
out_4832911148893244830[5] = 0;
|
||||
out_4832911148893244830[6] = 0;
|
||||
out_4832911148893244830[7] = 0;
|
||||
out_4832911148893244830[8] = 0;
|
||||
out_4832911148893244830[9] = 0;
|
||||
out_4832911148893244830[10] = 0;
|
||||
out_4832911148893244830[11] = 0;
|
||||
out_4832911148893244830[12] = 0;
|
||||
out_4832911148893244830[13] = 0;
|
||||
out_4832911148893244830[14] = 1;
|
||||
out_4832911148893244830[15] = 0;
|
||||
out_4832911148893244830[16] = 0;
|
||||
out_4832911148893244830[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_8535018277801169936) {
|
||||
out_8535018277801169936[0] = 0;
|
||||
out_8535018277801169936[1] = 0;
|
||||
out_8535018277801169936[2] = 0;
|
||||
out_8535018277801169936[3] = 0;
|
||||
out_8535018277801169936[4] = 1;
|
||||
out_8535018277801169936[5] = 0;
|
||||
out_8535018277801169936[6] = 0;
|
||||
out_8535018277801169936[7] = 0;
|
||||
out_8535018277801169936[8] = 0;
|
||||
out_8535018277801169936[9] = 0;
|
||||
out_8535018277801169936[10] = 0;
|
||||
out_8535018277801169936[11] = 0;
|
||||
out_8535018277801169936[12] = 0;
|
||||
out_8535018277801169936[13] = 0;
|
||||
out_8535018277801169936[14] = 1;
|
||||
out_8535018277801169936[15] = 0;
|
||||
out_8535018277801169936[16] = 0;
|
||||
out_8535018277801169936[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_7377917356997913735) {
|
||||
out_7377917356997913735[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_2021004951467369951) {
|
||||
out_2021004951467369951[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_8922850367303558593) {
|
||||
out_8922850367303558593[0] = 0;
|
||||
out_8922850367303558593[1] = 0;
|
||||
out_8922850367303558593[2] = 0;
|
||||
out_8922850367303558593[3] = 0;
|
||||
out_8922850367303558593[4] = 1;
|
||||
out_8922850367303558593[5] = 0;
|
||||
out_8922850367303558593[6] = 0;
|
||||
out_8922850367303558593[7] = 0;
|
||||
out_8922850367303558593[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_6996747533047273558) {
|
||||
out_6996747533047273558[0] = 0;
|
||||
out_6996747533047273558[1] = 0;
|
||||
out_6996747533047273558[2] = 0;
|
||||
out_6996747533047273558[3] = 0;
|
||||
out_6996747533047273558[4] = 1;
|
||||
out_6996747533047273558[5] = 0;
|
||||
out_6996747533047273558[6] = 0;
|
||||
out_6996747533047273558[7] = 0;
|
||||
out_6996747533047273558[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_5726959480825607318) {
|
||||
out_5726959480825607318[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_853261150391228214) {
|
||||
out_853261150391228214[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_3264057429024688172) {
|
||||
out_3264057429024688172[0] = 0;
|
||||
out_3264057429024688172[1] = 0;
|
||||
out_3264057429024688172[2] = 0;
|
||||
out_3264057429024688172[3] = 0;
|
||||
out_3264057429024688172[4] = 0;
|
||||
out_3264057429024688172[5] = 0;
|
||||
out_3264057429024688172[6] = 0;
|
||||
out_3264057429024688172[7] = 1;
|
||||
out_3264057429024688172[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_5190160263280973207) {
|
||||
out_5190160263280973207[0] = 0;
|
||||
out_5190160263280973207[1] = 0;
|
||||
out_5190160263280973207[2] = 0;
|
||||
out_5190160263280973207[3] = 0;
|
||||
out_5190160263280973207[4] = 0;
|
||||
out_5190160263280973207[5] = 0;
|
||||
out_5190160263280973207[6] = 0;
|
||||
out_5190160263280973207[7] = 1;
|
||||
out_5190160263280973207[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_703681777299558700) {
|
||||
out_703681777299558700[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_9074727769370462317) {
|
||||
out_9074727769370462317[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_7349130394605568112) {
|
||||
out_7349130394605568112[0] = 0;
|
||||
out_7349130394605568112[1] = 0;
|
||||
out_7349130394605568112[2] = 0;
|
||||
out_7349130394605568112[3] = 1;
|
||||
out_7349130394605568112[4] = 0;
|
||||
out_7349130394605568112[5] = 0;
|
||||
out_7349130394605568112[6] = 0;
|
||||
out_7349130394605568112[7] = 0;
|
||||
out_7349130394605568112[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_9171510844847698469) {
|
||||
out_9171510844847698469[0] = 0;
|
||||
out_9171510844847698469[1] = 0;
|
||||
out_9171510844847698469[2] = 0;
|
||||
out_9171510844847698469[3] = 1;
|
||||
out_9171510844847698469[4] = 0;
|
||||
out_9171510844847698469[5] = 0;
|
||||
out_9171510844847698469[6] = 0;
|
||||
out_9171510844847698469[7] = 0;
|
||||
out_9171510844847698469[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_4341131305553581050) {
|
||||
out_4341131305553581050[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_5628340221843964391) {
|
||||
out_5628340221843964391[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_8412619022989166409) {
|
||||
out_8412619022989166409[0] = 0;
|
||||
out_8412619022989166409[1] = 1;
|
||||
out_8412619022989166409[2] = 0;
|
||||
out_8412619022989166409[3] = 0;
|
||||
out_8412619022989166409[4] = 0;
|
||||
out_8412619022989166409[5] = 0;
|
||||
out_8412619022989166409[6] = 0;
|
||||
out_8412619022989166409[7] = 0;
|
||||
out_8412619022989166409[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_6486516188732881374) {
|
||||
out_6486516188732881374[0] = 0;
|
||||
out_6486516188732881374[1] = 1;
|
||||
out_6486516188732881374[2] = 0;
|
||||
out_6486516188732881374[3] = 0;
|
||||
out_6486516188732881374[4] = 0;
|
||||
out_6486516188732881374[5] = 0;
|
||||
out_6486516188732881374[6] = 0;
|
||||
out_6486516188732881374[7] = 0;
|
||||
out_6486516188732881374[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_8946752832143995776) {
|
||||
out_8946752832143995776[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_4073054501709616672) {
|
||||
out_4073054501709616672[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_4951726033650854633) {
|
||||
out_4951726033650854633[0] = 1;
|
||||
out_4951726033650854633[1] = 0;
|
||||
out_4951726033650854633[2] = 0;
|
||||
out_4951726033650854633[3] = 0;
|
||||
out_4951726033650854633[4] = 0;
|
||||
out_4951726033650854633[5] = 0;
|
||||
out_4951726033650854633[6] = 0;
|
||||
out_4951726033650854633[7] = 0;
|
||||
out_4951726033650854633[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_6877828867907139668) {
|
||||
out_6877828867907139668[0] = 1;
|
||||
out_6877828867907139668[1] = 0;
|
||||
out_6877828867907139668[2] = 0;
|
||||
out_6877828867907139668[3] = 0;
|
||||
out_6877828867907139668[4] = 0;
|
||||
out_6877828867907139668[5] = 0;
|
||||
out_6877828867907139668[6] = 0;
|
||||
out_6877828867907139668[7] = 0;
|
||||
out_6877828867907139668[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_3465273766459385496) {
|
||||
out_3465273766459385496[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_508659560249742002) {
|
||||
out_508659560249742002[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_2637849326791336696) {
|
||||
out_2637849326791336696[0] = 0;
|
||||
out_2637849326791336696[1] = 0;
|
||||
out_2637849326791336696[2] = 0;
|
||||
out_2637849326791336696[3] = 0;
|
||||
out_2637849326791336696[4] = 0;
|
||||
out_2637849326791336696[5] = 0;
|
||||
out_2637849326791336696[6] = 0;
|
||||
out_2637849326791336696[7] = 0;
|
||||
out_2637849326791336696[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_4563952161047621731) {
|
||||
out_4563952161047621731[0] = 0;
|
||||
out_4563952161047621731[1] = 0;
|
||||
out_4563952161047621731[2] = 0;
|
||||
out_4563952161047621731[3] = 0;
|
||||
out_4563952161047621731[4] = 0;
|
||||
out_4563952161047621731[5] = 0;
|
||||
out_4563952161047621731[6] = 0;
|
||||
out_4563952161047621731[7] = 0;
|
||||
out_4563952161047621731[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_287802830867790833) {
|
||||
err_fun(nom_x, delta_x, out_287802830867790833);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2092251631667009235) {
|
||||
err_fun(nom_x, delta_x, out_2092251631667009235);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1374850324761980275) {
|
||||
inv_err_fun(nom_x, true_x, out_1374850324761980275);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_2300837471234553583) {
|
||||
inv_err_fun(nom_x, true_x, out_2300837471234553583);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_6646352482882938111) {
|
||||
H_mod_fun(state, out_6646352482882938111);
|
||||
void car_H_mod_fun(double *state, double *out_4027971756970364591) {
|
||||
H_mod_fun(state, out_4027971756970364591);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_2311726086880249539) {
|
||||
f_fun(state, dt, out_2311726086880249539);
|
||||
void car_f_fun(double *state, double dt, double *out_6249326857146976875) {
|
||||
f_fun(state, dt, out_6249326857146976875);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_1431608970589135594) {
|
||||
F_fun(state, dt, out_1431608970589135594);
|
||||
void car_F_fun(double *state, double dt, double *out_1875491097223459739) {
|
||||
F_fun(state, dt, out_1875491097223459739);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_4297991490361286945) {
|
||||
h_25(state, unused, out_4297991490361286945);
|
||||
void car_h_25(double *state, double *unused, double *out_5658454479721392301) {
|
||||
h_25(state, unused, out_5658454479721392301);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_7005560747898744396) {
|
||||
H_25(state, unused, out_7005560747898744396);
|
||||
void car_H_25(double *state, double *unused, double *out_8931663582155029431) {
|
||||
H_25(state, unused, out_8931663582155029431);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_806466490562007731) {
|
||||
h_24(state, unused, out_806466490562007731);
|
||||
void car_h_24(double *state, double *unused, double *out_8388151047956984123) {
|
||||
h_24(state, unused, out_8388151047956984123);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_4832911148893244830) {
|
||||
H_24(state, unused, out_4832911148893244830);
|
||||
void car_H_24(double *state, double *unused, double *out_8535018277801169936) {
|
||||
H_24(state, unused, out_8535018277801169936);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_7377917356997913735) {
|
||||
h_30(state, unused, out_7377917356997913735);
|
||||
void car_h_30(double *state, double *unused, double *out_2021004951467369951) {
|
||||
h_30(state, unused, out_2021004951467369951);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_8922850367303558593) {
|
||||
H_30(state, unused, out_8922850367303558593);
|
||||
void car_H_30(double *state, double *unused, double *out_6996747533047273558) {
|
||||
H_30(state, unused, out_6996747533047273558);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_5726959480825607318) {
|
||||
h_26(state, unused, out_5726959480825607318);
|
||||
void car_h_26(double *state, double *unused, double *out_853261150391228214) {
|
||||
h_26(state, unused, out_853261150391228214);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_3264057429024688172) {
|
||||
H_26(state, unused, out_3264057429024688172);
|
||||
void car_H_26(double *state, double *unused, double *out_5190160263280973207) {
|
||||
H_26(state, unused, out_5190160263280973207);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_703681777299558700) {
|
||||
h_27(state, unused, out_703681777299558700);
|
||||
void car_h_27(double *state, double *unused, double *out_9074727769370462317) {
|
||||
h_27(state, unused, out_9074727769370462317);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_7349130394605568112) {
|
||||
H_27(state, unused, out_7349130394605568112);
|
||||
void car_H_27(double *state, double *unused, double *out_9171510844847698469) {
|
||||
H_27(state, unused, out_9171510844847698469);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_4341131305553581050) {
|
||||
h_29(state, unused, out_4341131305553581050);
|
||||
void car_h_29(double *state, double *unused, double *out_5628340221843964391) {
|
||||
h_29(state, unused, out_5628340221843964391);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_8412619022989166409) {
|
||||
H_29(state, unused, out_8412619022989166409);
|
||||
void car_H_29(double *state, double *unused, double *out_6486516188732881374) {
|
||||
H_29(state, unused, out_6486516188732881374);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_8946752832143995776) {
|
||||
h_28(state, unused, out_8946752832143995776);
|
||||
void car_h_28(double *state, double *unused, double *out_4073054501709616672) {
|
||||
h_28(state, unused, out_4073054501709616672);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_4951726033650854633) {
|
||||
H_28(state, unused, out_4951726033650854633);
|
||||
void car_H_28(double *state, double *unused, double *out_6877828867907139668) {
|
||||
H_28(state, unused, out_6877828867907139668);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_3465273766459385496) {
|
||||
h_31(state, unused, out_3465273766459385496);
|
||||
void car_h_31(double *state, double *unused, double *out_508659560249742002) {
|
||||
h_31(state, unused, out_508659560249742002);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_2637849326791336696) {
|
||||
H_31(state, unused, out_2637849326791336696);
|
||||
void car_H_31(double *state, double *unused, double *out_4563952161047621731) {
|
||||
H_31(state, unused, out_4563952161047621731);
|
||||
}
|
||||
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_287802830867790833);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1374850324761980275);
|
||||
void car_H_mod_fun(double *state, double *out_6646352482882938111);
|
||||
void car_f_fun(double *state, double dt, double *out_2311726086880249539);
|
||||
void car_F_fun(double *state, double dt, double *out_1431608970589135594);
|
||||
void car_h_25(double *state, double *unused, double *out_4297991490361286945);
|
||||
void car_H_25(double *state, double *unused, double *out_7005560747898744396);
|
||||
void car_h_24(double *state, double *unused, double *out_806466490562007731);
|
||||
void car_H_24(double *state, double *unused, double *out_4832911148893244830);
|
||||
void car_h_30(double *state, double *unused, double *out_7377917356997913735);
|
||||
void car_H_30(double *state, double *unused, double *out_8922850367303558593);
|
||||
void car_h_26(double *state, double *unused, double *out_5726959480825607318);
|
||||
void car_H_26(double *state, double *unused, double *out_3264057429024688172);
|
||||
void car_h_27(double *state, double *unused, double *out_703681777299558700);
|
||||
void car_H_27(double *state, double *unused, double *out_7349130394605568112);
|
||||
void car_h_29(double *state, double *unused, double *out_4341131305553581050);
|
||||
void car_H_29(double *state, double *unused, double *out_8412619022989166409);
|
||||
void car_h_28(double *state, double *unused, double *out_8946752832143995776);
|
||||
void car_H_28(double *state, double *unused, double *out_4951726033650854633);
|
||||
void car_h_31(double *state, double *unused, double *out_3465273766459385496);
|
||||
void car_H_31(double *state, double *unused, double *out_2637849326791336696);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2092251631667009235);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_2300837471234553583);
|
||||
void car_H_mod_fun(double *state, double *out_4027971756970364591);
|
||||
void car_f_fun(double *state, double dt, double *out_6249326857146976875);
|
||||
void car_F_fun(double *state, double dt, double *out_1875491097223459739);
|
||||
void car_h_25(double *state, double *unused, double *out_5658454479721392301);
|
||||
void car_H_25(double *state, double *unused, double *out_8931663582155029431);
|
||||
void car_h_24(double *state, double *unused, double *out_8388151047956984123);
|
||||
void car_H_24(double *state, double *unused, double *out_8535018277801169936);
|
||||
void car_h_30(double *state, double *unused, double *out_2021004951467369951);
|
||||
void car_H_30(double *state, double *unused, double *out_6996747533047273558);
|
||||
void car_h_26(double *state, double *unused, double *out_853261150391228214);
|
||||
void car_H_26(double *state, double *unused, double *out_5190160263280973207);
|
||||
void car_h_27(double *state, double *unused, double *out_9074727769370462317);
|
||||
void car_H_27(double *state, double *unused, double *out_9171510844847698469);
|
||||
void car_h_29(double *state, double *unused, double *out_5628340221843964391);
|
||||
void car_H_29(double *state, double *unused, double *out_6486516188732881374);
|
||||
void car_h_28(double *state, double *unused, double *out_4073054501709616672);
|
||||
void car_H_28(double *state, double *unused, double *out_6877828867907139668);
|
||||
void car_h_31(double *state, double *unused, double *out_508659560249742002);
|
||||
void car_H_31(double *state, double *unused, double *out_4563952161047621731);
|
||||
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);
|
||||
|
||||
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_1773490464841058211);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3370820179269337821);
|
||||
void pose_H_mod_fun(double *state, double *out_8103172798179963867);
|
||||
void pose_f_fun(double *state, double dt, double *out_4130745264458481066);
|
||||
void pose_F_fun(double *state, double dt, double *out_8817991526926240656);
|
||||
void pose_h_4(double *state, double *unused, double *out_7394028133800952799);
|
||||
void pose_H_4(double *state, double *unused, double *out_6283474072273667006);
|
||||
void pose_h_10(double *state, double *unused, double *out_1632777674839787006);
|
||||
void pose_H_10(double *state, double *unused, double *out_761013637887660448);
|
||||
void pose_h_13(double *state, double *unused, double *out_4603793361074312483);
|
||||
void pose_H_13(double *state, double *unused, double *out_8950996176103551809);
|
||||
void pose_h_14(double *state, double *unused, double *out_5241449068668224266);
|
||||
void pose_H_14(double *state, double *unused, double *out_3200685639978294710);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_7150506142011136089);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4307514453378045812);
|
||||
void pose_H_mod_fun(double *state, double *out_526209948270695177);
|
||||
void pose_f_fun(double *state, double dt, double *out_826153410981652937);
|
||||
void pose_F_fun(double *state, double dt, double *out_2504584572758712159);
|
||||
void pose_h_4(double *state, double *unused, double *out_7442480903303367172);
|
||||
void pose_H_4(double *state, double *unused, double *out_3800432027061644430);
|
||||
void pose_h_10(double *state, double *unused, double *out_8932011361837519248);
|
||||
void pose_H_10(double *state, double *unused, double *out_4464178208942290131);
|
||||
void pose_h_13(double *state, double *unused, double *out_6459110219088517442);
|
||||
void pose_H_13(double *state, double *unused, double *out_588158201729311629);
|
||||
void pose_h_14(double *state, double *unused, double *out_7025019117935111535);
|
||||
void pose_H_14(double *state, double *unused, double *out_162808829277840099);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
@@ -2023,6 +2023,17 @@
|
||||
"data_type": "bool",
|
||||
"ui_type": "toggle"
|
||||
},
|
||||
{
|
||||
"key": "ClearNavOnOffroadTimeoutMinutes",
|
||||
"label": "Clear Route Delay",
|
||||
"description": "How long the device can stay offroad before clearing the active route. Set to 0 for immediate clearing.",
|
||||
"data_type": "int",
|
||||
"ui_type": "numeric",
|
||||
"min": 0,
|
||||
"max": 180,
|
||||
"step": 1,
|
||||
"parent_key": "ClearNavOnOffroad"
|
||||
},
|
||||
{
|
||||
"key": "RoadNameUI",
|
||||
"label": "Road Name",
|
||||
|
||||
Binary file not shown.
File diff suppressed because one or more lines are too long
@@ -1,9 +1,9 @@
|
||||
#include "cdm.h"
|
||||
#include "stddef.h"
|
||||
|
||||
int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel) {
|
||||
int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode) {
|
||||
struct cdm_dmi_cmd *cmd = (struct cdm_dmi_cmd*)dst;
|
||||
cmd->cmd = CAM_CDM_CMD_DMI_32;
|
||||
cmd->cmd = opcode;
|
||||
cmd->length = length - 1;
|
||||
cmd->reserved = 0;
|
||||
cmd->addr = 0; // gets patched in
|
||||
|
||||
@@ -6,11 +6,6 @@
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
// our helpers
|
||||
int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
|
||||
int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
|
||||
int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel);
|
||||
|
||||
// from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
|
||||
|
||||
enum cam_cdm_command {
|
||||
@@ -32,6 +27,11 @@ enum cam_cdm_command {
|
||||
CAM_CDM_CMD_PRIVATE_BASE_MAX = 0x7F
|
||||
};
|
||||
|
||||
// our helpers
|
||||
int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
|
||||
int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
|
||||
int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode = CAM_CDM_CMD_DMI_32);
|
||||
|
||||
/**
|
||||
* struct cdm_regrandom_cmd - Definition for CDM random register command.
|
||||
* @count: Number of register writes
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
#include <cassert>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/mman.h>
|
||||
#include <tuple>
|
||||
|
||||
#include "media/cam_defs.h"
|
||||
#include "media/cam_isp.h"
|
||||
@@ -21,6 +22,21 @@
|
||||
#include "system/camerad/cameras/bps_blobs.h"
|
||||
|
||||
|
||||
// Returns NV12 aligned (stride, y_height, uv_height, buffer_size) for the given frame dimensions.
|
||||
static std::tuple<uint32_t, uint32_t, uint32_t, uint32_t> get_nv12_info(int width, int height) {
|
||||
const uint32_t stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width);
|
||||
const uint32_t y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, height);
|
||||
const uint32_t uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, height);
|
||||
const uint32_t size = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height);
|
||||
|
||||
assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, width));
|
||||
assert(y_height / 2 == uv_height);
|
||||
assert((stride * y_height) % 0x1000 == 0);
|
||||
|
||||
return {stride, y_height, uv_height, size};
|
||||
}
|
||||
|
||||
|
||||
// ************** low level camera helpers ****************
|
||||
|
||||
int do_cam_control(int fd, int op_code, void *handle, int size) {
|
||||
@@ -481,8 +497,11 @@ void SpectraCamera::config_bps(int idx, int request_id) {
|
||||
* BPS = Bayer Processing Segment
|
||||
*/
|
||||
|
||||
int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2 + sizeof(struct cam_buf_io_cfg)*2;
|
||||
size += sizeof(struct cam_patch_desc)*9;
|
||||
bool needs_downscale = sensor->out_scale > 1;
|
||||
int num_io_cfgs = needs_downscale ? 3 : 2;
|
||||
int num_patches = needs_downscale ? 14 : 12;
|
||||
int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2 + sizeof(struct cam_buf_io_cfg)*num_io_cfgs;
|
||||
size += sizeof(struct cam_patch_desc)*num_patches;
|
||||
|
||||
uint32_t cam_packet_handle = 0;
|
||||
auto pkt = m->mem_mgr.alloc<struct cam_packet>(size, &cam_packet_handle);
|
||||
@@ -560,8 +579,15 @@ void SpectraCamera::config_bps(int idx, int request_id) {
|
||||
int cdm_len = 0;
|
||||
|
||||
if (bps_lin_reg.size() == 0) {
|
||||
// set first knee pt to do BLC
|
||||
uint32_t new_knee[8];
|
||||
new_knee[0] = sensor->black_level << (14 - sensor->bits_per_pixel);
|
||||
for (int i = 0; i < 7; i++) {
|
||||
uint32_t pts = sensor->linearization_pts[i / 2];
|
||||
new_knee[i + 1] = (i % 2 == 0) ? (pts >> 16) : (pts & 0xffff);
|
||||
}
|
||||
for (int i = 0; i < 4; i++) {
|
||||
bps_lin_reg.push_back(((sensor->linearization_pts[i] & 0xffff) << 0x10) | (sensor->linearization_pts[i] >> 0x10));
|
||||
bps_lin_reg.push_back((new_knee[2*i + 1] << 16) | new_knee[2*i]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -584,20 +610,24 @@ void SpectraCamera::config_bps(int idx, int request_id) {
|
||||
0x00000080,
|
||||
0x00800066,
|
||||
});
|
||||
// linearization, EN=0
|
||||
// linearization
|
||||
cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1868, bps_lin_reg);
|
||||
cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1878, bps_lin_reg);
|
||||
cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1888, bps_lin_reg);
|
||||
cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1898, bps_lin_reg);
|
||||
/*
|
||||
uint8_t *start = (unsigned char *)bps_cdm_program_array.ptr + cdm_len;
|
||||
uint64_t addr;
|
||||
cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->linearization_lut.size()*sizeof(uint32_t), 0x1808, 1);
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
*/
|
||||
cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->linearization_lut.size()*sizeof(uint32_t), 0x1808, 1, CAM_CDM_CMD_DMI);
|
||||
patches.push_back(addr - (uint64_t)bps_cdm_program_array.ptr);
|
||||
|
||||
// color correction
|
||||
cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2e68, bps_ccm_reg);
|
||||
|
||||
// gamma
|
||||
for (uint8_t ch = 1; ch <= 3; ch++) {
|
||||
cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->gamma_lut_rgb.size()*sizeof(uint32_t), 0x3208, ch, CAM_CDM_CMD_DMI);
|
||||
patches.push_back(addr - (uint64_t)bps_cdm_program_array.ptr);
|
||||
}
|
||||
|
||||
cdm_len += build_common_ife_bps((unsigned char *)bps_cdm_program_array.ptr + cdm_len, cc, sensor.get(), patches, false);
|
||||
|
||||
pa->length = cdm_len - 1;
|
||||
@@ -611,7 +641,7 @@ void SpectraCamera::config_bps(int idx, int request_id) {
|
||||
tmp.header = CAM_ICP_CMD_GENERIC_BLOB_CLK;
|
||||
tmp.header |= (sizeof(cam_icp_clk_bw_request)) << 8;
|
||||
tmp.clk.budget_ns = 0x1fca058;
|
||||
tmp.clk.frame_cycles = 2329024; // comes from the striping lib
|
||||
tmp.clk.frame_cycles = sensor->frame_width * sensor->frame_height; // matches striping lib pixelCount
|
||||
tmp.clk.rt_flag = 0x0;
|
||||
tmp.clk.uncompressed_bw = 0x38512180;
|
||||
tmp.clk.compressed_bw = 0x38512180;
|
||||
@@ -626,7 +656,7 @@ void SpectraCamera::config_bps(int idx, int request_id) {
|
||||
}
|
||||
|
||||
// *** io config ***
|
||||
pkt->num_io_configs = 2;
|
||||
pkt->num_io_configs = num_io_cfgs;
|
||||
pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
|
||||
struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
|
||||
{
|
||||
@@ -670,28 +700,69 @@ void SpectraCamera::config_bps(int idx, int request_id) {
|
||||
|
||||
io_cfg[1].format = CAM_FORMAT_NV12; // TODO: why is this 21 in the dump? should be 12
|
||||
io_cfg[1].color_space = CAM_COLOR_SPACE_BT601_FULL;
|
||||
io_cfg[1].resource_type = CAM_ICP_BPS_OUTPUT_IMAGE_FULL;
|
||||
io_cfg[1].resource_type = needs_downscale ? CAM_ICP_BPS_OUTPUT_IMAGE_REG1 : CAM_ICP_BPS_OUTPUT_IMAGE_FULL;
|
||||
io_cfg[1].fence = sync_objs_bps[idx];
|
||||
io_cfg[1].direction = CAM_BUF_OUTPUT;
|
||||
io_cfg[1].subsample_pattern = 0x1;
|
||||
io_cfg[1].framedrop_pattern = 0x1;
|
||||
|
||||
if (needs_downscale) {
|
||||
// downscaling needs a full res placeholder
|
||||
uint32_t full_stride, full_y_h, full_uv_h, full_yuv_size;
|
||||
std::tie(full_stride, full_y_h, full_uv_h, full_yuv_size) = get_nv12_info(sensor->frame_width, sensor->frame_height);
|
||||
io_cfg[2].mem_handle[0] = bps_fullres_dummy.handle;
|
||||
io_cfg[2].mem_handle[1] = bps_fullres_dummy.handle;
|
||||
io_cfg[2].planes[0] = (struct cam_plane_cfg){
|
||||
.width = sensor->frame_width,
|
||||
.height = sensor->frame_height,
|
||||
.plane_stride = full_stride,
|
||||
.slice_height = full_y_h,
|
||||
};
|
||||
io_cfg[2].planes[1] = (struct cam_plane_cfg){
|
||||
.width = sensor->frame_width,
|
||||
.height = sensor->frame_height / 2,
|
||||
.plane_stride = full_stride,
|
||||
.slice_height = full_uv_h,
|
||||
};
|
||||
io_cfg[2].offsets[1] = ALIGNED_SIZE(full_stride * full_y_h, 0x1000);
|
||||
io_cfg[2].format = CAM_FORMAT_NV12;
|
||||
io_cfg[2].color_space = CAM_COLOR_SPACE_BT601_FULL;
|
||||
io_cfg[2].resource_type = CAM_ICP_BPS_OUTPUT_IMAGE_FULL;
|
||||
io_cfg[2].fence = sync_objs_bps[idx];
|
||||
io_cfg[2].direction = CAM_BUF_OUTPUT;
|
||||
io_cfg[2].subsample_pattern = 0x1;
|
||||
io_cfg[2].framedrop_pattern = 0x1;
|
||||
}
|
||||
}
|
||||
|
||||
// *** patches ***
|
||||
{
|
||||
assert(patches.size() == 0 | patches.size() == 1);
|
||||
assert(patches.size() == 0 || patches.size() == 4);
|
||||
pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs;
|
||||
|
||||
if (patches.size() > 0) {
|
||||
add_patch(pkt.get(), bps_cmd.handle, patches[0], bps_linearization_lut.handle, 0);
|
||||
// linearization LUT
|
||||
add_patch(pkt.get(), bps_cdm_program_array.handle, patches[0], bps_linearization_lut.handle, 0);
|
||||
// gamma LUTs
|
||||
for (int i = 0; i < 3; i++) {
|
||||
add_patch(pkt.get(), bps_cdm_program_array.handle, patches[i+1], bps_gamma_lut.handle, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// input frame
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[0].ptr[0]), buf_handle_raw[idx], 0);
|
||||
|
||||
// output frame
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), buf_handle_yuv[idx], 0);
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]);
|
||||
if (needs_downscale) {
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), bps_fullres_dummy.handle, 0);
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), bps_fullres_dummy.handle, io_cfg[2].offsets[1]);
|
||||
// output frame at REG1
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[7].ptr[0]), buf_handle_yuv[idx], 0);
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[7].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]);
|
||||
} else {
|
||||
// output frame at FULL
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), buf_handle_yuv[idx], 0);
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]);
|
||||
}
|
||||
|
||||
// rest of buffers
|
||||
add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, settings_addr), bps_iq.handle, 0);
|
||||
@@ -1002,9 +1073,6 @@ bool SpectraCamera::openSensor() {
|
||||
LOGD("-- Probing sensor %d", cc.camera_num);
|
||||
|
||||
auto init_sensor_lambda = [this](SensorInfo *s) {
|
||||
if (s->image_sensor == cereal::FrameData::ImageSensor::OS04C10 && cc.output_type == ISP_IFE_PROCESSED) {
|
||||
((OS04C10*)s)->ife_downscale_configure();
|
||||
}
|
||||
sensor.reset(s);
|
||||
return (sensors_init() == 0);
|
||||
};
|
||||
@@ -1183,13 +1251,39 @@ void SpectraCamera::configICP() {
|
||||
|
||||
// used internally by the BPS, we just allocate it.
|
||||
// size comes from the BPSStripingLib
|
||||
bps_cdm_striping_bl.init(m, 0xa100, 0x20, true, m->icp_device_iommu);
|
||||
bps_cdm_striping_bl.init(m, 0xcfe0, 0x20, true, m->icp_device_iommu);
|
||||
|
||||
if (sensor->out_scale > 1) {
|
||||
uint32_t full_stride, full_y_h, full_uv_h, full_yuv_size;
|
||||
std::tie(full_stride, full_y_h, full_uv_h, full_yuv_size) = get_nv12_info(sensor->frame_width, sensor->frame_height);
|
||||
bps_fullres_dummy.init(m, full_yuv_size, 0x1000, true, m->icp_device_iommu);
|
||||
}
|
||||
|
||||
// LUTs
|
||||
/*
|
||||
assert(sensor->linearization_lut.size() == 36);
|
||||
bps_linearization_lut.init(m, sensor->linearization_lut.size()*sizeof(uint32_t), 0x20, true, m->icp_device_iommu);
|
||||
memcpy(bps_linearization_lut.ptr, sensor->linearization_lut.data(), bps_linearization_lut.size);
|
||||
*/
|
||||
|
||||
// bit shift linearization_lut to bps specs, also compensate for black level here
|
||||
uint32_t bl = sensor->black_level << (14 - sensor->bits_per_pixel);
|
||||
uint32_t* bps_lut = (uint32_t*)bps_linearization_lut.ptr;
|
||||
for (size_t i = 0; i < sensor->linearization_lut.size(); i++) {
|
||||
size_t seg = i / 4;
|
||||
size_t ch = i % 4;
|
||||
if (seg == 0) {
|
||||
bps_lut[i] = 0;
|
||||
continue;
|
||||
}
|
||||
uint32_t e = sensor->linearization_lut[(seg - 1) * 4 + ch];
|
||||
uint32_t base = e & 0x3fff;
|
||||
uint32_t slope_q11 = (e >> 14) & 0x3fff;
|
||||
uint32_t slope_q12 = std::min<uint32_t>(slope_q11 << 1, 0x3fff);
|
||||
base = (base > bl) ? (base - bl) : 0;
|
||||
bps_lut[i] = base | (slope_q12 << 14);
|
||||
}
|
||||
|
||||
assert(sensor->gamma_lut_rgb.size() == 64);
|
||||
bps_gamma_lut.init(m, sensor->gamma_lut_rgb.size()*sizeof(uint32_t), 0x20, true, m->icp_device_iommu);
|
||||
memcpy(bps_gamma_lut.ptr, sensor->gamma_lut_rgb.data(), bps_gamma_lut.size);
|
||||
}
|
||||
|
||||
void SpectraCamera::configCSIPHY() {
|
||||
|
||||
@@ -173,6 +173,8 @@ public:
|
||||
SpectraBuf bps_iq;
|
||||
SpectraBuf bps_striping;
|
||||
SpectraBuf bps_linearization_lut;
|
||||
SpectraBuf bps_gamma_lut;
|
||||
SpectraBuf bps_fullres_dummy;
|
||||
std::vector<uint32_t> bps_lin_reg;
|
||||
std::vector<uint32_t> bps_ccm_reg;
|
||||
|
||||
|
||||
@@ -21,27 +21,16 @@ const uint32_t os04c10_analog_gains_reg[] = {
|
||||
|
||||
} // namespace
|
||||
|
||||
void OS04C10::ife_downscale_configure() {
|
||||
out_scale = 2;
|
||||
|
||||
pixel_size_mm = 0.002;
|
||||
frame_width = 2688;
|
||||
frame_height = 1520;
|
||||
exposure_time_max = 2352;
|
||||
|
||||
init_reg_array.insert(init_reg_array.end(), std::begin(ife_downscale_override_array_os04c10), std::end(ife_downscale_override_array_os04c10));
|
||||
}
|
||||
|
||||
OS04C10::OS04C10() {
|
||||
image_sensor = cereal::FrameData::ImageSensor::OS04C10;
|
||||
bayer_pattern = CAM_ISP_PATTERN_BAYER_BGBGBG;
|
||||
pixel_size_mm = 0.004;
|
||||
pixel_size_mm = 0.002;
|
||||
data_word = false;
|
||||
|
||||
// hdr_offset = 64 * 2 + 8; // stagger
|
||||
frame_width = 1344;
|
||||
frame_height = 760; //760 * 2 + hdr_offset;
|
||||
frame_stride = (frame_width * 12 / 8); // no alignment
|
||||
out_scale = 2;
|
||||
frame_width = 2688;
|
||||
frame_height = 1520;
|
||||
frame_stride = frame_width * 12 / 8;
|
||||
|
||||
extra_height = 0;
|
||||
frame_offset = 0;
|
||||
@@ -65,7 +54,7 @@ OS04C10::OS04C10() {
|
||||
dc_gain_on_grey = 0.9;
|
||||
dc_gain_off_grey = 1.0;
|
||||
exposure_time_min = 2;
|
||||
exposure_time_max = 1684;
|
||||
exposure_time_max = 2352;
|
||||
analog_gain_min_idx = 0x0;
|
||||
analog_gain_rec_idx = 0x0; // 1x
|
||||
analog_gain_max_idx = 0x28;
|
||||
|
||||
@@ -4,7 +4,7 @@ const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}};
|
||||
const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}};
|
||||
|
||||
const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
// baseed on DP_2688X1520_NEWSTG_MIPI0776Mbps_30FPS_10BIT_FOURLANE
|
||||
// OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz
|
||||
{0x0103, 0x01}, // software reset
|
||||
|
||||
// PLL + clocks
|
||||
@@ -93,7 +93,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
{0x388b, 0x00},
|
||||
{0x3c80, 0x10},
|
||||
{0x3c86, 0x00},
|
||||
{0x3c8c, 0x20},
|
||||
{0x3c8c, 0x40},
|
||||
{0x3c9f, 0x01},
|
||||
{0x3d85, 0x1b},
|
||||
{0x3d8c, 0x71},
|
||||
@@ -197,7 +197,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
{0x370b, 0x48},
|
||||
{0x370c, 0x01},
|
||||
{0x370f, 0x00},
|
||||
{0x3714, 0x28},
|
||||
{0x3714, 0x24},
|
||||
{0x3716, 0x04},
|
||||
{0x3719, 0x11},
|
||||
{0x371a, 0x1e},
|
||||
@@ -229,7 +229,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
{0x37bd, 0x01},
|
||||
{0x37bf, 0x26},
|
||||
{0x37c0, 0x11},
|
||||
{0x37c2, 0x14},
|
||||
{0x37c2, 0x04},
|
||||
{0x37cd, 0x19},
|
||||
{0x37e0, 0x08},
|
||||
{0x37e6, 0x04},
|
||||
@@ -239,14 +239,14 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
{0x37d8, 0x02},
|
||||
{0x37e2, 0x10},
|
||||
{0x3739, 0x10},
|
||||
{0x3662, 0x08},
|
||||
{0x3662, 0x10},
|
||||
{0x37e4, 0x20},
|
||||
{0x37e3, 0x08},
|
||||
{0x37d9, 0x04},
|
||||
{0x37d9, 0x08},
|
||||
{0x4040, 0x00},
|
||||
{0x4041, 0x03},
|
||||
{0x4008, 0x01},
|
||||
{0x4009, 0x06},
|
||||
{0x4041, 0x07},
|
||||
{0x4008, 0x02},
|
||||
{0x4009, 0x0d},
|
||||
|
||||
// FSIN - frame sync
|
||||
{0x3002, 0x22},
|
||||
@@ -267,20 +267,20 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
{0x3802, 0x00}, {0x3803, 0x00},
|
||||
{0x3804, 0x0a}, {0x3805, 0x8f},
|
||||
{0x3806, 0x05}, {0x3807, 0xff},
|
||||
{0x3808, 0x05}, {0x3809, 0x40},
|
||||
{0x380a, 0x02}, {0x380b, 0xf8},
|
||||
{0x3808, 0x0a}, {0x3809, 0x80},
|
||||
{0x380a, 0x05}, {0x380b, 0xf0},
|
||||
{0x3811, 0x08},
|
||||
{0x3813, 0x08},
|
||||
{0x3814, 0x03},
|
||||
{0x3814, 0x01},
|
||||
{0x3815, 0x01},
|
||||
{0x3816, 0x03},
|
||||
{0x3816, 0x01},
|
||||
{0x3817, 0x01},
|
||||
|
||||
{0x380c, 0x0b}, {0x380d, 0xac}, // HTS (line length)
|
||||
{0x380e, 0x06}, {0x380f, 0x9c}, // VTS (frame length)
|
||||
{0x380c, 0x08}, {0x380d, 0x5c}, // HTS (line length)
|
||||
{0x380e, 0x09}, {0x380f, 0x38}, // VTS (frame length)
|
||||
|
||||
{0x3820, 0xb3},
|
||||
{0x3821, 0x01},
|
||||
{0x3820, 0xb0},
|
||||
{0x3821, 0x00},
|
||||
{0x3880, 0x00},
|
||||
{0x3882, 0x20},
|
||||
{0x3c91, 0x0b},
|
||||
@@ -330,23 +330,3 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
{0x5104, 0x08}, {0x5105, 0xd6},
|
||||
{0x5144, 0x08}, {0x5145, 0xd6},
|
||||
};
|
||||
|
||||
const struct i2c_random_wr_payload ife_downscale_override_array_os04c10[] = {
|
||||
// based on OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz
|
||||
{0x3c8c, 0x40},
|
||||
{0x3714, 0x24},
|
||||
{0x37c2, 0x04},
|
||||
{0x3662, 0x10},
|
||||
{0x37d9, 0x08},
|
||||
{0x4041, 0x07},
|
||||
{0x4008, 0x02},
|
||||
{0x4009, 0x0d},
|
||||
{0x3808, 0x0a}, {0x3809, 0x80},
|
||||
{0x380a, 0x05}, {0x380b, 0xf0},
|
||||
{0x3814, 0x01},
|
||||
{0x3816, 0x01},
|
||||
{0x380c, 0x08}, {0x380d, 0x5c}, // HTS
|
||||
{0x380e, 0x09}, {0x380f, 0x38}, // VTS
|
||||
{0x3820, 0xb0},
|
||||
{0x3821, 0x00},
|
||||
};
|
||||
|
||||
@@ -110,7 +110,6 @@ public:
|
||||
class OS04C10 : public SensorInfo {
|
||||
public:
|
||||
OS04C10();
|
||||
void ife_downscale_configure();
|
||||
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
|
||||
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
|
||||
int getSlaveAddress(int port) const override;
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -70,6 +70,46 @@ def _is_numeric_stat_value(value) -> bool:
|
||||
return isinstance(value, (int, float)) and not isinstance(value, bool)
|
||||
|
||||
|
||||
def _get_int_param_value(params, key: str, default: int = 0) -> int:
|
||||
getter = getattr(params, "get_int", None)
|
||||
if callable(getter):
|
||||
try:
|
||||
return int(getter(key))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
value = params.get(key)
|
||||
if value is None:
|
||||
return default
|
||||
if isinstance(value, bytes):
|
||||
value = value.decode("utf-8", errors="ignore")
|
||||
|
||||
try:
|
||||
return int(float(str(value).strip()))
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def get_nav_offroad_clear_timeout_seconds(params) -> int:
|
||||
return max(_get_int_param_value(params, "ClearNavOnOffroadTimeoutMinutes", 0), 0) * 60
|
||||
|
||||
|
||||
def update_nav_offroad_clear_state(params, started: bool, tracked_destination, tracked_started_at, now: float):
|
||||
nav_destination = params.get("NavDestination")
|
||||
if started or not params.get_bool("ClearNavOnOffroad") or not nav_destination:
|
||||
return None, None
|
||||
|
||||
if nav_destination != tracked_destination or tracked_started_at is None:
|
||||
tracked_destination = nav_destination
|
||||
tracked_started_at = now
|
||||
|
||||
if now - tracked_started_at >= get_nav_offroad_clear_timeout_seconds(params):
|
||||
params.remove("NavDestination")
|
||||
return None, None
|
||||
|
||||
return tracked_destination, tracked_started_at
|
||||
|
||||
|
||||
def _merge_starpilot_stat_values(existing, incoming, key=None):
|
||||
if existing is None:
|
||||
return incoming
|
||||
@@ -765,6 +805,8 @@ def manager_thread() -> None:
|
||||
started_prev = False
|
||||
ignition_prev = False
|
||||
warned_onroad_reboot = False
|
||||
offroad_nav_destination = None
|
||||
offroad_nav_started_at = None
|
||||
|
||||
# StarPilot variables
|
||||
sm = sm.extend(['starpilotPlan'])
|
||||
@@ -788,8 +830,10 @@ def manager_thread() -> None:
|
||||
|
||||
# StarPilot variables
|
||||
params_memory.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION)
|
||||
if params.get_bool("ClearNavOnOffroad"):
|
||||
params.remove("NavDestination")
|
||||
|
||||
offroad_nav_destination, offroad_nav_started_at = update_nav_offroad_clear_state(
|
||||
params, started, offroad_nav_destination, offroad_nav_started_at, time.monotonic()
|
||||
)
|
||||
|
||||
ignition = any(ps.ignitionLine or ps.ignitionCan for ps in sm['pandaStates'] if ps.pandaType != log.PandaState.PandaType.unknown)
|
||||
if ignition and not ignition_prev:
|
||||
|
||||
Reference in New Issue
Block a user