Let it drink the water from the stream

2026-08-06 00:36:25 +08:00 · 2026-08-02 12:41:25 -05:00
parent d89371c518
commit 2175217e9b
100 changed files with 1888 additions and 1460 deletions
@@ -70,6 +70,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"IsDriverViewEnabled", {CLEAR_ON_MANAGER_START, BOOL}},
    {"IsEngaged", {PERSISTENT, BOOL}},
    {"IsLdwEnabled", {PERSISTENT, BOOL}},
+    {"IsLiveStreaming", {CLEAR_ON_MANAGER_START, BOOL}},
    {"IsMetric", {PERSISTENT, BOOL}},
    {"IsOffroad", {CLEAR_ON_MANAGER_START, BOOL}},
    {"IsOnroad", {PERSISTENT, BOOL}},
@@ -94,6 +95,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"LiveDelay", {PERSISTENT, BYTES}},
    {"LiveParameters", {PERSISTENT, JSON}},
    {"LiveParametersV2", {PERSISTENT, BYTES}},
+    {"LivestreamEncoderBitrate", {CLEAR_ON_MANAGER_START | DONT_LOG, INT}},
+    {"LivestreamRequestKeyframe", {CLEAR_ON_MANAGER_START | DONT_LOG, BOOL}},
    {"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}},
    {"LateralManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
    {"LocationFilterInitialState", {PERSISTENT, BYTES}},
@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-6d4089fe-DEBUG";
+const uint8_t gitversion[19] = "DEV-d89371c5-DEBUG";
@@ -1 +1 @@
-DEV-6d4089fe-DEBUG
+DEV-d89371c5-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_3645428350016324042) {
-   out_3645428350016324042[0] = delta_x[0] + nom_x[0];
-   out_3645428350016324042[1] = delta_x[1] + nom_x[1];
-   out_3645428350016324042[2] = delta_x[2] + nom_x[2];
-   out_3645428350016324042[3] = delta_x[3] + nom_x[3];
-   out_3645428350016324042[4] = delta_x[4] + nom_x[4];
-   out_3645428350016324042[5] = delta_x[5] + nom_x[5];
-   out_3645428350016324042[6] = delta_x[6] + nom_x[6];
-   out_3645428350016324042[7] = delta_x[7] + nom_x[7];
-   out_3645428350016324042[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_3928386937016580969) {
+   out_3928386937016580969[0] = delta_x[0] + nom_x[0];
+   out_3928386937016580969[1] = delta_x[1] + nom_x[1];
+   out_3928386937016580969[2] = delta_x[2] + nom_x[2];
+   out_3928386937016580969[3] = delta_x[3] + nom_x[3];
+   out_3928386937016580969[4] = delta_x[4] + nom_x[4];
+   out_3928386937016580969[5] = delta_x[5] + nom_x[5];
+   out_3928386937016580969[6] = delta_x[6] + nom_x[6];
+   out_3928386937016580969[7] = delta_x[7] + nom_x[7];
+   out_3928386937016580969[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_6760420506725333180) {
-   out_6760420506725333180[0] = -nom_x[0] + true_x[0];
-   out_6760420506725333180[1] = -nom_x[1] + true_x[1];
-   out_6760420506725333180[2] = -nom_x[2] + true_x[2];
-   out_6760420506725333180[3] = -nom_x[3] + true_x[3];
-   out_6760420506725333180[4] = -nom_x[4] + true_x[4];
-   out_6760420506725333180[5] = -nom_x[5] + true_x[5];
-   out_6760420506725333180[6] = -nom_x[6] + true_x[6];
-   out_6760420506725333180[7] = -nom_x[7] + true_x[7];
-   out_6760420506725333180[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_5072261536488927858) {
+   out_5072261536488927858[0] = -nom_x[0] + true_x[0];
+   out_5072261536488927858[1] = -nom_x[1] + true_x[1];
+   out_5072261536488927858[2] = -nom_x[2] + true_x[2];
+   out_5072261536488927858[3] = -nom_x[3] + true_x[3];
+   out_5072261536488927858[4] = -nom_x[4] + true_x[4];
+   out_5072261536488927858[5] = -nom_x[5] + true_x[5];
+   out_5072261536488927858[6] = -nom_x[6] + true_x[6];
+   out_5072261536488927858[7] = -nom_x[7] + true_x[7];
+   out_5072261536488927858[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_2494414039273921066) {
-   out_2494414039273921066[0] = 1.0;
-   out_2494414039273921066[1] = 0.0;
-   out_2494414039273921066[2] = 0.0;
-   out_2494414039273921066[3] = 0.0;
-   out_2494414039273921066[4] = 0.0;
-   out_2494414039273921066[5] = 0.0;
-   out_2494414039273921066[6] = 0.0;
-   out_2494414039273921066[7] = 0.0;
-   out_2494414039273921066[8] = 0.0;
-   out_2494414039273921066[9] = 0.0;
-   out_2494414039273921066[10] = 1.0;
-   out_2494414039273921066[11] = 0.0;
-   out_2494414039273921066[12] = 0.0;
-   out_2494414039273921066[13] = 0.0;
-   out_2494414039273921066[14] = 0.0;
-   out_2494414039273921066[15] = 0.0;
-   out_2494414039273921066[16] = 0.0;
-   out_2494414039273921066[17] = 0.0;
-   out_2494414039273921066[18] = 0.0;
-   out_2494414039273921066[19] = 0.0;
-   out_2494414039273921066[20] = 1.0;
-   out_2494414039273921066[21] = 0.0;
-   out_2494414039273921066[22] = 0.0;
-   out_2494414039273921066[23] = 0.0;
-   out_2494414039273921066[24] = 0.0;
-   out_2494414039273921066[25] = 0.0;
-   out_2494414039273921066[26] = 0.0;
-   out_2494414039273921066[27] = 0.0;
-   out_2494414039273921066[28] = 0.0;
-   out_2494414039273921066[29] = 0.0;
-   out_2494414039273921066[30] = 1.0;
-   out_2494414039273921066[31] = 0.0;
-   out_2494414039273921066[32] = 0.0;
-   out_2494414039273921066[33] = 0.0;
-   out_2494414039273921066[34] = 0.0;
-   out_2494414039273921066[35] = 0.0;
-   out_2494414039273921066[36] = 0.0;
-   out_2494414039273921066[37] = 0.0;
-   out_2494414039273921066[38] = 0.0;
-   out_2494414039273921066[39] = 0.0;
-   out_2494414039273921066[40] = 1.0;
-   out_2494414039273921066[41] = 0.0;
-   out_2494414039273921066[42] = 0.0;
-   out_2494414039273921066[43] = 0.0;
-   out_2494414039273921066[44] = 0.0;
-   out_2494414039273921066[45] = 0.0;
-   out_2494414039273921066[46] = 0.0;
-   out_2494414039273921066[47] = 0.0;
-   out_2494414039273921066[48] = 0.0;
-   out_2494414039273921066[49] = 0.0;
-   out_2494414039273921066[50] = 1.0;
-   out_2494414039273921066[51] = 0.0;
-   out_2494414039273921066[52] = 0.0;
-   out_2494414039273921066[53] = 0.0;
-   out_2494414039273921066[54] = 0.0;
-   out_2494414039273921066[55] = 0.0;
-   out_2494414039273921066[56] = 0.0;
-   out_2494414039273921066[57] = 0.0;
-   out_2494414039273921066[58] = 0.0;
-   out_2494414039273921066[59] = 0.0;
-   out_2494414039273921066[60] = 1.0;
-   out_2494414039273921066[61] = 0.0;
-   out_2494414039273921066[62] = 0.0;
-   out_2494414039273921066[63] = 0.0;
-   out_2494414039273921066[64] = 0.0;
-   out_2494414039273921066[65] = 0.0;
-   out_2494414039273921066[66] = 0.0;
-   out_2494414039273921066[67] = 0.0;
-   out_2494414039273921066[68] = 0.0;
-   out_2494414039273921066[69] = 0.0;
-   out_2494414039273921066[70] = 1.0;
-   out_2494414039273921066[71] = 0.0;
-   out_2494414039273921066[72] = 0.0;
-   out_2494414039273921066[73] = 0.0;
-   out_2494414039273921066[74] = 0.0;
-   out_2494414039273921066[75] = 0.0;
-   out_2494414039273921066[76] = 0.0;
-   out_2494414039273921066[77] = 0.0;
-   out_2494414039273921066[78] = 0.0;
-   out_2494414039273921066[79] = 0.0;
-   out_2494414039273921066[80] = 1.0;
+void H_mod_fun(double *state, double *out_1805157416881491113) {
+   out_1805157416881491113[0] = 1.0;
+   out_1805157416881491113[1] = 0.0;
+   out_1805157416881491113[2] = 0.0;
+   out_1805157416881491113[3] = 0.0;
+   out_1805157416881491113[4] = 0.0;
+   out_1805157416881491113[5] = 0.0;
+   out_1805157416881491113[6] = 0.0;
+   out_1805157416881491113[7] = 0.0;
+   out_1805157416881491113[8] = 0.0;
+   out_1805157416881491113[9] = 0.0;
+   out_1805157416881491113[10] = 1.0;
+   out_1805157416881491113[11] = 0.0;
+   out_1805157416881491113[12] = 0.0;
+   out_1805157416881491113[13] = 0.0;
+   out_1805157416881491113[14] = 0.0;
+   out_1805157416881491113[15] = 0.0;
+   out_1805157416881491113[16] = 0.0;
+   out_1805157416881491113[17] = 0.0;
+   out_1805157416881491113[18] = 0.0;
+   out_1805157416881491113[19] = 0.0;
+   out_1805157416881491113[20] = 1.0;
+   out_1805157416881491113[21] = 0.0;
+   out_1805157416881491113[22] = 0.0;
+   out_1805157416881491113[23] = 0.0;
+   out_1805157416881491113[24] = 0.0;
+   out_1805157416881491113[25] = 0.0;
+   out_1805157416881491113[26] = 0.0;
+   out_1805157416881491113[27] = 0.0;
+   out_1805157416881491113[28] = 0.0;
+   out_1805157416881491113[29] = 0.0;
+   out_1805157416881491113[30] = 1.0;
+   out_1805157416881491113[31] = 0.0;
+   out_1805157416881491113[32] = 0.0;
+   out_1805157416881491113[33] = 0.0;
+   out_1805157416881491113[34] = 0.0;
+   out_1805157416881491113[35] = 0.0;
+   out_1805157416881491113[36] = 0.0;
+   out_1805157416881491113[37] = 0.0;
+   out_1805157416881491113[38] = 0.0;
+   out_1805157416881491113[39] = 0.0;
+   out_1805157416881491113[40] = 1.0;
+   out_1805157416881491113[41] = 0.0;
+   out_1805157416881491113[42] = 0.0;
+   out_1805157416881491113[43] = 0.0;
+   out_1805157416881491113[44] = 0.0;
+   out_1805157416881491113[45] = 0.0;
+   out_1805157416881491113[46] = 0.0;
+   out_1805157416881491113[47] = 0.0;
+   out_1805157416881491113[48] = 0.0;
+   out_1805157416881491113[49] = 0.0;
+   out_1805157416881491113[50] = 1.0;
+   out_1805157416881491113[51] = 0.0;
+   out_1805157416881491113[52] = 0.0;
+   out_1805157416881491113[53] = 0.0;
+   out_1805157416881491113[54] = 0.0;
+   out_1805157416881491113[55] = 0.0;
+   out_1805157416881491113[56] = 0.0;
+   out_1805157416881491113[57] = 0.0;
+   out_1805157416881491113[58] = 0.0;
+   out_1805157416881491113[59] = 0.0;
+   out_1805157416881491113[60] = 1.0;
+   out_1805157416881491113[61] = 0.0;
+   out_1805157416881491113[62] = 0.0;
+   out_1805157416881491113[63] = 0.0;
+   out_1805157416881491113[64] = 0.0;
+   out_1805157416881491113[65] = 0.0;
+   out_1805157416881491113[66] = 0.0;
+   out_1805157416881491113[67] = 0.0;
+   out_1805157416881491113[68] = 0.0;
+   out_1805157416881491113[69] = 0.0;
+   out_1805157416881491113[70] = 1.0;
+   out_1805157416881491113[71] = 0.0;
+   out_1805157416881491113[72] = 0.0;
+   out_1805157416881491113[73] = 0.0;
+   out_1805157416881491113[74] = 0.0;
+   out_1805157416881491113[75] = 0.0;
+   out_1805157416881491113[76] = 0.0;
+   out_1805157416881491113[77] = 0.0;
+   out_1805157416881491113[78] = 0.0;
+   out_1805157416881491113[79] = 0.0;
+   out_1805157416881491113[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_7248228104803536253) {
-   out_7248228104803536253[0] = state[0];
-   out_7248228104803536253[1] = state[1];
-   out_7248228104803536253[2] = state[2];
-   out_7248228104803536253[3] = state[3];
-   out_7248228104803536253[4] = state[4];
-   out_7248228104803536253[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_7248228104803536253[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_7248228104803536253[7] = state[7];
-   out_7248228104803536253[8] = state[8];
+void f_fun(double *state, double dt, double *out_779920196187082914) {
+   out_779920196187082914[0] = state[0];
+   out_779920196187082914[1] = state[1];
+   out_779920196187082914[2] = state[2];
+   out_779920196187082914[3] = state[3];
+   out_779920196187082914[4] = state[4];
+   out_779920196187082914[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_779920196187082914[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_779920196187082914[7] = state[7];
+   out_779920196187082914[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_8524982651096840533) {
-   out_8524982651096840533[0] = 1;
-   out_8524982651096840533[1] = 0;
-   out_8524982651096840533[2] = 0;
-   out_8524982651096840533[3] = 0;
-   out_8524982651096840533[4] = 0;
-   out_8524982651096840533[5] = 0;
-   out_8524982651096840533[6] = 0;
-   out_8524982651096840533[7] = 0;
-   out_8524982651096840533[8] = 0;
-   out_8524982651096840533[9] = 0;
-   out_8524982651096840533[10] = 1;
-   out_8524982651096840533[11] = 0;
-   out_8524982651096840533[12] = 0;
-   out_8524982651096840533[13] = 0;
-   out_8524982651096840533[14] = 0;
-   out_8524982651096840533[15] = 0;
-   out_8524982651096840533[16] = 0;
-   out_8524982651096840533[17] = 0;
-   out_8524982651096840533[18] = 0;
-   out_8524982651096840533[19] = 0;
-   out_8524982651096840533[20] = 1;
-   out_8524982651096840533[21] = 0;
-   out_8524982651096840533[22] = 0;
-   out_8524982651096840533[23] = 0;
-   out_8524982651096840533[24] = 0;
-   out_8524982651096840533[25] = 0;
-   out_8524982651096840533[26] = 0;
-   out_8524982651096840533[27] = 0;
-   out_8524982651096840533[28] = 0;
-   out_8524982651096840533[29] = 0;
-   out_8524982651096840533[30] = 1;
-   out_8524982651096840533[31] = 0;
-   out_8524982651096840533[32] = 0;
-   out_8524982651096840533[33] = 0;
-   out_8524982651096840533[34] = 0;
-   out_8524982651096840533[35] = 0;
-   out_8524982651096840533[36] = 0;
-   out_8524982651096840533[37] = 0;
-   out_8524982651096840533[38] = 0;
-   out_8524982651096840533[39] = 0;
-   out_8524982651096840533[40] = 1;
-   out_8524982651096840533[41] = 0;
-   out_8524982651096840533[42] = 0;
-   out_8524982651096840533[43] = 0;
-   out_8524982651096840533[44] = 0;
-   out_8524982651096840533[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_8524982651096840533[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_8524982651096840533[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_8524982651096840533[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_8524982651096840533[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_8524982651096840533[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_8524982651096840533[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_8524982651096840533[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_8524982651096840533[53] = -9.8100000000000005*dt;
-   out_8524982651096840533[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_8524982651096840533[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_8524982651096840533[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_8524982651096840533[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_8524982651096840533[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_8524982651096840533[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_8524982651096840533[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_8524982651096840533[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_8524982651096840533[62] = 0;
-   out_8524982651096840533[63] = 0;
-   out_8524982651096840533[64] = 0;
-   out_8524982651096840533[65] = 0;
-   out_8524982651096840533[66] = 0;
-   out_8524982651096840533[67] = 0;
-   out_8524982651096840533[68] = 0;
-   out_8524982651096840533[69] = 0;
-   out_8524982651096840533[70] = 1;
-   out_8524982651096840533[71] = 0;
-   out_8524982651096840533[72] = 0;
-   out_8524982651096840533[73] = 0;
-   out_8524982651096840533[74] = 0;
-   out_8524982651096840533[75] = 0;
-   out_8524982651096840533[76] = 0;
-   out_8524982651096840533[77] = 0;
-   out_8524982651096840533[78] = 0;
-   out_8524982651096840533[79] = 0;
-   out_8524982651096840533[80] = 1;
+void F_fun(double *state, double dt, double *out_3629707451146971735) {
+   out_3629707451146971735[0] = 1;
+   out_3629707451146971735[1] = 0;
+   out_3629707451146971735[2] = 0;
+   out_3629707451146971735[3] = 0;
+   out_3629707451146971735[4] = 0;
+   out_3629707451146971735[5] = 0;
+   out_3629707451146971735[6] = 0;
+   out_3629707451146971735[7] = 0;
+   out_3629707451146971735[8] = 0;
+   out_3629707451146971735[9] = 0;
+   out_3629707451146971735[10] = 1;
+   out_3629707451146971735[11] = 0;
+   out_3629707451146971735[12] = 0;
+   out_3629707451146971735[13] = 0;
+   out_3629707451146971735[14] = 0;
+   out_3629707451146971735[15] = 0;
+   out_3629707451146971735[16] = 0;
+   out_3629707451146971735[17] = 0;
+   out_3629707451146971735[18] = 0;
+   out_3629707451146971735[19] = 0;
+   out_3629707451146971735[20] = 1;
+   out_3629707451146971735[21] = 0;
+   out_3629707451146971735[22] = 0;
+   out_3629707451146971735[23] = 0;
+   out_3629707451146971735[24] = 0;
+   out_3629707451146971735[25] = 0;
+   out_3629707451146971735[26] = 0;
+   out_3629707451146971735[27] = 0;
+   out_3629707451146971735[28] = 0;
+   out_3629707451146971735[29] = 0;
+   out_3629707451146971735[30] = 1;
+   out_3629707451146971735[31] = 0;
+   out_3629707451146971735[32] = 0;
+   out_3629707451146971735[33] = 0;
+   out_3629707451146971735[34] = 0;
+   out_3629707451146971735[35] = 0;
+   out_3629707451146971735[36] = 0;
+   out_3629707451146971735[37] = 0;
+   out_3629707451146971735[38] = 0;
+   out_3629707451146971735[39] = 0;
+   out_3629707451146971735[40] = 1;
+   out_3629707451146971735[41] = 0;
+   out_3629707451146971735[42] = 0;
+   out_3629707451146971735[43] = 0;
+   out_3629707451146971735[44] = 0;
+   out_3629707451146971735[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_3629707451146971735[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_3629707451146971735[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3629707451146971735[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3629707451146971735[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_3629707451146971735[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_3629707451146971735[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_3629707451146971735[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3629707451146971735[53] = -9.8100000000000005*dt;
+   out_3629707451146971735[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_3629707451146971735[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_3629707451146971735[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3629707451146971735[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3629707451146971735[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_3629707451146971735[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_3629707451146971735[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_3629707451146971735[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3629707451146971735[62] = 0;
+   out_3629707451146971735[63] = 0;
+   out_3629707451146971735[64] = 0;
+   out_3629707451146971735[65] = 0;
+   out_3629707451146971735[66] = 0;
+   out_3629707451146971735[67] = 0;
+   out_3629707451146971735[68] = 0;
+   out_3629707451146971735[69] = 0;
+   out_3629707451146971735[70] = 1;
+   out_3629707451146971735[71] = 0;
+   out_3629707451146971735[72] = 0;
+   out_3629707451146971735[73] = 0;
+   out_3629707451146971735[74] = 0;
+   out_3629707451146971735[75] = 0;
+   out_3629707451146971735[76] = 0;
+   out_3629707451146971735[77] = 0;
+   out_3629707451146971735[78] = 0;
+   out_3629707451146971735[79] = 0;
+   out_3629707451146971735[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_8901373037917505693) {
-   out_8901373037917505693[0] = state[6];
+void h_25(double *state, double *unused, double *out_3062657207957604668) {
+   out_3062657207957604668[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_6698774186638316171) {
-   out_6698774186638316171[0] = 0;
-   out_6698774186638316171[1] = 0;
-   out_6698774186638316171[2] = 0;
-   out_6698774186638316171[3] = 0;
-   out_6698774186638316171[4] = 0;
-   out_6698774186638316171[5] = 0;
-   out_6698774186638316171[6] = 1;
-   out_6698774186638316171[7] = 0;
-   out_6698774186638316171[8] = 0;
+void H_25(double *state, double *unused, double *out_6170957893956391582) {
+   out_6170957893956391582[0] = 0;
+   out_6170957893956391582[1] = 0;
+   out_6170957893956391582[2] = 0;
+   out_6170957893956391582[3] = 0;
+   out_6170957893956391582[4] = 0;
+   out_6170957893956391582[5] = 0;
+   out_6170957893956391582[6] = 1;
+   out_6170957893956391582[7] = 0;
+   out_6170957893956391582[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_4440266777255545355) {
-   out_4440266777255545355[0] = state[4];
-   out_4440266777255545355[1] = state[5];
+void h_24(double *state, double *unused, double *out_4363661235373079824) {
+   out_4363661235373079824[0] = state[4];
+   out_4363661235373079824[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_2529290999430879054) {
-   out_2529290999430879054[0] = 0;
-   out_2529290999430879054[1] = 0;
-   out_2529290999430879054[2] = 0;
-   out_2529290999430879054[3] = 0;
-   out_2529290999430879054[4] = 1;
-   out_2529290999430879054[5] = 0;
-   out_2529290999430879054[6] = 0;
-   out_2529290999430879054[7] = 0;
-   out_2529290999430879054[8] = 0;
-   out_2529290999430879054[9] = 0;
-   out_2529290999430879054[10] = 0;
-   out_2529290999430879054[11] = 0;
-   out_2529290999430879054[12] = 0;
-   out_2529290999430879054[13] = 0;
-   out_2529290999430879054[14] = 1;
-   out_2529290999430879054[15] = 0;
-   out_2529290999430879054[16] = 0;
-   out_2529290999430879054[17] = 0;
+void H_24(double *state, double *unused, double *out_6464400548730383069) {
+   out_6464400548730383069[0] = 0;
+   out_6464400548730383069[1] = 0;
+   out_6464400548730383069[2] = 0;
+   out_6464400548730383069[3] = 0;
+   out_6464400548730383069[4] = 1;
+   out_6464400548730383069[5] = 0;
+   out_6464400548730383069[6] = 0;
+   out_6464400548730383069[7] = 0;
+   out_6464400548730383069[8] = 0;
+   out_6464400548730383069[9] = 0;
+   out_6464400548730383069[10] = 0;
+   out_6464400548730383069[11] = 0;
+   out_6464400548730383069[12] = 0;
+   out_6464400548730383069[13] = 0;
+   out_6464400548730383069[14] = 1;
+   out_6464400548730383069[15] = 0;
+   out_6464400548730383069[16] = 0;
+   out_6464400548730383069[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_261495561198963809) {
-   out_261495561198963809[0] = state[4];
+void h_30(double *state, double *unused, double *out_8192537752896435152) {
+   out_8192537752896435152[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_7220273556943627247) {
-   out_7220273556943627247[0] = 0;
-   out_7220273556943627247[1] = 0;
-   out_7220273556943627247[2] = 0;
-   out_7220273556943627247[3] = 0;
-   out_7220273556943627247[4] = 1;
-   out_7220273556943627247[5] = 0;
-   out_7220273556943627247[6] = 0;
-   out_7220273556943627247[7] = 0;
-   out_7220273556943627247[8] = 0;
+void H_30(double *state, double *unused, double *out_5359095838261543279) {
+   out_5359095838261543279[0] = 0;
+   out_5359095838261543279[1] = 0;
+   out_5359095838261543279[2] = 0;
+   out_5359095838261543279[3] = 0;
+   out_5359095838261543279[4] = 1;
+   out_5359095838261543279[5] = 0;
+   out_5359095838261543279[6] = 0;
+   out_5359095838261543279[7] = 0;
+   out_5359095838261543279[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_4740177706461881836) {
-   out_4740177706461881836[0] = state[7];
+void h_26(double *state, double *unused, double *out_1793496235482586006) {
+   out_1793496235482586006[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_8006466568197179221) {
-   out_8006466568197179221[0] = 0;
-   out_8006466568197179221[1] = 0;
-   out_8006466568197179221[2] = 0;
-   out_8006466568197179221[3] = 0;
-   out_8006466568197179221[4] = 0;
-   out_8006466568197179221[5] = 0;
-   out_8006466568197179221[6] = 0;
-   out_8006466568197179221[7] = 1;
-   out_8006466568197179221[8] = 0;
+void H_26(double *state, double *unused, double *out_8971260209992359433) {
+   out_8971260209992359433[0] = 0;
+   out_8971260209992359433[1] = 0;
+   out_8971260209992359433[2] = 0;
+   out_8971260209992359433[3] = 0;
+   out_8971260209992359433[4] = 0;
+   out_8971260209992359433[5] = 0;
+   out_8971260209992359433[6] = 0;
+   out_8971260209992359433[7] = 1;
+   out_8971260209992359433[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_6088820934647652310) {
-   out_6088820934647652310[0] = state[3];
+void h_27(double *state, double *unused, double *out_8064330475618450313) {
+   out_8064330475618450313[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_9002876445581981152) {
-   out_9002876445581981152[0] = 0;
-   out_9002876445581981152[1] = 0;
-   out_9002876445581981152[2] = 0;
-   out_9002876445581981152[3] = 1;
-   out_9002876445581981152[4] = 0;
-   out_9002876445581981152[5] = 0;
-   out_9002876445581981152[6] = 0;
-   out_9002876445581981152[7] = 0;
-   out_9002876445581981152[8] = 0;
+void H_27(double *state, double *unused, double *out_7533859150061968190) {
+   out_7533859150061968190[0] = 0;
+   out_7533859150061968190[1] = 0;
+   out_7533859150061968190[2] = 0;
+   out_7533859150061968190[3] = 1;
+   out_7533859150061968190[4] = 0;
+   out_7533859150061968190[5] = 0;
+   out_7533859150061968190[6] = 0;
+   out_7533859150061968190[7] = 0;
+   out_7533859150061968190[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_906248239877177928) {
-   out_906248239877177928[0] = state[1];
+void h_29(double *state, double *unused, double *out_6613699082989567146) {
+   out_6613699082989567146[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_7730504901258019431) {
-   out_7730504901258019431[0] = 0;
-   out_7730504901258019431[1] = 1;
-   out_7730504901258019431[2] = 0;
-   out_7730504901258019431[3] = 0;
-   out_7730504901258019431[4] = 0;
-   out_7730504901258019431[5] = 0;
-   out_7730504901258019431[6] = 0;
-   out_7730504901258019431[7] = 0;
-   out_7730504901258019431[8] = 0;
+void H_29(double *state, double *unused, double *out_4848864493947151095) {
+   out_4848864493947151095[0] = 0;
+   out_4848864493947151095[1] = 1;
+   out_4848864493947151095[2] = 0;
+   out_4848864493947151095[3] = 0;
+   out_4848864493947151095[4] = 0;
+   out_4848864493947151095[5] = 0;
+   out_4848864493947151095[6] = 0;
+   out_4848864493947151095[7] = 0;
+   out_4848864493947151095[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_3561613674795902166) {
-   out_3561613674795902166[0] = state[0];
+void h_28(double *state, double *unused, double *out_7120820622716787878) {
+   out_7120820622716787878[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_2648105884188488857) {
-   out_2648105884188488857[0] = 1;
-   out_2648105884188488857[1] = 0;
-   out_2648105884188488857[2] = 0;
-   out_2648105884188488857[3] = 0;
-   out_2648105884188488857[4] = 0;
-   out_2648105884188488857[5] = 0;
-   out_2648105884188488857[6] = 0;
-   out_2648105884188488857[7] = 0;
-   out_2648105884188488857[8] = 0;
+void H_28(double *state, double *unused, double *out_4117123179708501819) {
+   out_4117123179708501819[0] = 1;
+   out_4117123179708501819[1] = 0;
+   out_4117123179708501819[2] = 0;
+   out_4117123179708501819[3] = 0;
+   out_4117123179708501819[4] = 0;
+   out_4117123179708501819[5] = 0;
+   out_4117123179708501819[6] = 0;
+   out_4117123179708501819[7] = 0;
+   out_4117123179708501819[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_8626178975632999804) {
-   out_8626178975632999804[0] = state[8];
+void h_31(double *state, double *unused, double *out_574792320296417682) {
+   out_574792320296417682[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_6668128224761355743) {
-   out_6668128224761355743[0] = 0;
-   out_6668128224761355743[1] = 0;
-   out_6668128224761355743[2] = 0;
-   out_6668128224761355743[3] = 0;
-   out_6668128224761355743[4] = 0;
-   out_6668128224761355743[5] = 0;
-   out_6668128224761355743[6] = 0;
-   out_6668128224761355743[7] = 0;
-   out_6668128224761355743[8] = 1;
+void H_31(double *state, double *unused, double *out_5199110929241342781) {
+   out_5199110929241342781[0] = 0;
+   out_5199110929241342781[1] = 0;
+   out_5199110929241342781[2] = 0;
+   out_5199110929241342781[3] = 0;
+   out_5199110929241342781[4] = 0;
+   out_5199110929241342781[5] = 0;
+   out_5199110929241342781[6] = 0;
+   out_5199110929241342781[7] = 0;
+   out_5199110929241342781[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_3645428350016324042) {
-  err_fun(nom_x, delta_x, out_3645428350016324042);
+void car_err_fun(double *nom_x, double *delta_x, double *out_3928386937016580969) {
+  err_fun(nom_x, delta_x, out_3928386937016580969);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6760420506725333180) {
-  inv_err_fun(nom_x, true_x, out_6760420506725333180);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_5072261536488927858) {
+  inv_err_fun(nom_x, true_x, out_5072261536488927858);
 }
-void car_H_mod_fun(double *state, double *out_2494414039273921066) {
-  H_mod_fun(state, out_2494414039273921066);
+void car_H_mod_fun(double *state, double *out_1805157416881491113) {
+  H_mod_fun(state, out_1805157416881491113);
 }
-void car_f_fun(double *state, double dt, double *out_7248228104803536253) {
-  f_fun(state,  dt, out_7248228104803536253);
+void car_f_fun(double *state, double dt, double *out_779920196187082914) {
+  f_fun(state,  dt, out_779920196187082914);
 }
-void car_F_fun(double *state, double dt, double *out_8524982651096840533) {
-  F_fun(state,  dt, out_8524982651096840533);
+void car_F_fun(double *state, double dt, double *out_3629707451146971735) {
+  F_fun(state,  dt, out_3629707451146971735);
 }
-void car_h_25(double *state, double *unused, double *out_8901373037917505693) {
-  h_25(state, unused, out_8901373037917505693);
+void car_h_25(double *state, double *unused, double *out_3062657207957604668) {
+  h_25(state, unused, out_3062657207957604668);
 }
-void car_H_25(double *state, double *unused, double *out_6698774186638316171) {
-  H_25(state, unused, out_6698774186638316171);
+void car_H_25(double *state, double *unused, double *out_6170957893956391582) {
+  H_25(state, unused, out_6170957893956391582);
 }
-void car_h_24(double *state, double *unused, double *out_4440266777255545355) {
-  h_24(state, unused, out_4440266777255545355);
+void car_h_24(double *state, double *unused, double *out_4363661235373079824) {
+  h_24(state, unused, out_4363661235373079824);
 }
-void car_H_24(double *state, double *unused, double *out_2529290999430879054) {
-  H_24(state, unused, out_2529290999430879054);
+void car_H_24(double *state, double *unused, double *out_6464400548730383069) {
+  H_24(state, unused, out_6464400548730383069);
 }
-void car_h_30(double *state, double *unused, double *out_261495561198963809) {
-  h_30(state, unused, out_261495561198963809);
+void car_h_30(double *state, double *unused, double *out_8192537752896435152) {
+  h_30(state, unused, out_8192537752896435152);
 }
-void car_H_30(double *state, double *unused, double *out_7220273556943627247) {
-  H_30(state, unused, out_7220273556943627247);
+void car_H_30(double *state, double *unused, double *out_5359095838261543279) {
+  H_30(state, unused, out_5359095838261543279);
 }
-void car_h_26(double *state, double *unused, double *out_4740177706461881836) {
-  h_26(state, unused, out_4740177706461881836);
+void car_h_26(double *state, double *unused, double *out_1793496235482586006) {
+  h_26(state, unused, out_1793496235482586006);
 }
-void car_H_26(double *state, double *unused, double *out_8006466568197179221) {
-  H_26(state, unused, out_8006466568197179221);
+void car_H_26(double *state, double *unused, double *out_8971260209992359433) {
+  H_26(state, unused, out_8971260209992359433);
 }
-void car_h_27(double *state, double *unused, double *out_6088820934647652310) {
-  h_27(state, unused, out_6088820934647652310);
+void car_h_27(double *state, double *unused, double *out_8064330475618450313) {
+  h_27(state, unused, out_8064330475618450313);
 }
-void car_H_27(double *state, double *unused, double *out_9002876445581981152) {
-  H_27(state, unused, out_9002876445581981152);
+void car_H_27(double *state, double *unused, double *out_7533859150061968190) {
+  H_27(state, unused, out_7533859150061968190);
 }
-void car_h_29(double *state, double *unused, double *out_906248239877177928) {
-  h_29(state, unused, out_906248239877177928);
+void car_h_29(double *state, double *unused, double *out_6613699082989567146) {
+  h_29(state, unused, out_6613699082989567146);
 }
-void car_H_29(double *state, double *unused, double *out_7730504901258019431) {
-  H_29(state, unused, out_7730504901258019431);
+void car_H_29(double *state, double *unused, double *out_4848864493947151095) {
+  H_29(state, unused, out_4848864493947151095);
 }
-void car_h_28(double *state, double *unused, double *out_3561613674795902166) {
-  h_28(state, unused, out_3561613674795902166);
+void car_h_28(double *state, double *unused, double *out_7120820622716787878) {
+  h_28(state, unused, out_7120820622716787878);
 }
-void car_H_28(double *state, double *unused, double *out_2648105884188488857) {
-  H_28(state, unused, out_2648105884188488857);
+void car_H_28(double *state, double *unused, double *out_4117123179708501819) {
+  H_28(state, unused, out_4117123179708501819);
 }
-void car_h_31(double *state, double *unused, double *out_8626178975632999804) {
-  h_31(state, unused, out_8626178975632999804);
+void car_h_31(double *state, double *unused, double *out_574792320296417682) {
+  h_31(state, unused, out_574792320296417682);
 }
-void car_H_31(double *state, double *unused, double *out_6668128224761355743) {
-  H_31(state, unused, out_6668128224761355743);
+void car_H_31(double *state, double *unused, double *out_5199110929241342781) {
+  H_31(state, unused, out_5199110929241342781);
 }
 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_3645428350016324042);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6760420506725333180);
-void car_H_mod_fun(double *state, double *out_2494414039273921066);
-void car_f_fun(double *state, double dt, double *out_7248228104803536253);
-void car_F_fun(double *state, double dt, double *out_8524982651096840533);
-void car_h_25(double *state, double *unused, double *out_8901373037917505693);
-void car_H_25(double *state, double *unused, double *out_6698774186638316171);
-void car_h_24(double *state, double *unused, double *out_4440266777255545355);
-void car_H_24(double *state, double *unused, double *out_2529290999430879054);
-void car_h_30(double *state, double *unused, double *out_261495561198963809);
-void car_H_30(double *state, double *unused, double *out_7220273556943627247);
-void car_h_26(double *state, double *unused, double *out_4740177706461881836);
-void car_H_26(double *state, double *unused, double *out_8006466568197179221);
-void car_h_27(double *state, double *unused, double *out_6088820934647652310);
-void car_H_27(double *state, double *unused, double *out_9002876445581981152);
-void car_h_29(double *state, double *unused, double *out_906248239877177928);
-void car_H_29(double *state, double *unused, double *out_7730504901258019431);
-void car_h_28(double *state, double *unused, double *out_3561613674795902166);
-void car_H_28(double *state, double *unused, double *out_2648105884188488857);
-void car_h_31(double *state, double *unused, double *out_8626178975632999804);
-void car_H_31(double *state, double *unused, double *out_6668128224761355743);
+void car_err_fun(double *nom_x, double *delta_x, double *out_3928386937016580969);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_5072261536488927858);
+void car_H_mod_fun(double *state, double *out_1805157416881491113);
+void car_f_fun(double *state, double dt, double *out_779920196187082914);
+void car_F_fun(double *state, double dt, double *out_3629707451146971735);
+void car_h_25(double *state, double *unused, double *out_3062657207957604668);
+void car_H_25(double *state, double *unused, double *out_6170957893956391582);
+void car_h_24(double *state, double *unused, double *out_4363661235373079824);
+void car_H_24(double *state, double *unused, double *out_6464400548730383069);
+void car_h_30(double *state, double *unused, double *out_8192537752896435152);
+void car_H_30(double *state, double *unused, double *out_5359095838261543279);
+void car_h_26(double *state, double *unused, double *out_1793496235482586006);
+void car_H_26(double *state, double *unused, double *out_8971260209992359433);
+void car_h_27(double *state, double *unused, double *out_8064330475618450313);
+void car_H_27(double *state, double *unused, double *out_7533859150061968190);
+void car_h_29(double *state, double *unused, double *out_6613699082989567146);
+void car_H_29(double *state, double *unused, double *out_4848864493947151095);
+void car_h_28(double *state, double *unused, double *out_7120820622716787878);
+void car_H_28(double *state, double *unused, double *out_4117123179708501819);
+void car_h_31(double *state, double *unused, double *out_574792320296417682);
+void car_H_31(double *state, double *unused, double *out_5199110929241342781);
 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);
@@ -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_2394949464519988932);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6906452154256624155);
-void pose_H_mod_fun(double *state, double *out_4838410850925167920);
-void pose_f_fun(double *state, double dt, double *out_8751525120696163789);
-void pose_F_fun(double *state, double dt, double *out_7175128552495691270);
-void pose_h_4(double *state, double *unused, double *out_1724585592717515492);
-void pose_H_4(double *state, double *unused, double *out_3018712125018871059);
-void pose_h_10(double *state, double *unused, double *out_1452906161636095115);
-void pose_H_10(double *state, double *unused, double *out_3099904708442444122);
-void pose_h_13(double *state, double *unused, double *out_7528264878139249656);
-void pose_H_13(double *state, double *unused, double *out_3583314044700715163);
-void pose_h_14(double *state, double *unused, double *out_4413124467477602634);
-void pose_H_14(double *state, double *unused, double *out_64076307276501237);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_9137514370372247790);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3476070474607186610);
+void pose_H_mod_fun(double *state, double *out_2374218852650606410);
+void pose_f_fun(double *state, double dt, double *out_4392889107205501252);
+void pose_F_fun(double *state, double dt, double *out_5161498735714983758);
+void pose_h_4(double *state, double *unused, double *out_6700070712992729001);
+void pose_H_4(double *state, double *unused, double *out_554520126744309549);
+void pose_h_10(double *state, double *unused, double *out_326755883218172788);
+void pose_H_10(double *state, double *unused, double *out_3618077715207225770);
+void pose_h_13(double *state, double *unused, double *out_8852328989829848020);
+void pose_H_13(double *state, double *unused, double *out_3279235336558214475);
+void pose_h_14(double *state, double *unused, double *out_2099262646432106248);
+void pose_H_14(double *state, double *unused, double *out_2528268305551062747);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -28,7 +28,7 @@ from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutExce
                       create_connection)

 import cereal.messaging as messaging
-from cereal import log
+from cereal import car, log
 from cereal.services import SERVICE_LIST
 from openpilot.common.api import Api, get_key_pair
 from openpilot.common.utils import CallbackReader, get_upload_stream
@@ -565,6 +565,34 @@ def getNetworks():
  return HARDWARE.get_networks()


+@dispatcher.add_method
+def startStream(sdp: str, enabled: bool) -> dict:
+  from openpilot.system.webrtc.helpers import StreamRequestBody, post_stream_request, wait_for_webrtcd
+
+  params = Params()
+  bridge_services_in = []
+
+  # The manager shuts webrtcd down on ignition transitions, so persistent car
+  # params are safe here and remain available while the device is offroad.
+  cp_bytes = params.get("CarParamsPersistent")
+  if cp_bytes is None:
+    raise Exception("failed to get CarParamsPersistent")
+
+  with car.CarParams.from_bytes(cp_bytes) as CP:
+    if CP.notCar:
+      bridge_services_in.append("testJoystick")
+
+  if params.get_bool("IsOffroad"):
+    # Wake camerad, stream_encoderd, and webrtcd. webrtcd clears this after the
+    # last session ends, allowing manager to stop the extra processes again.
+    params.put_bool("IsLiveStreaming", True)
+    wait_for_webrtcd()
+
+  return post_stream_request(StreamRequestBody(
+    sdp, "wideRoad", enabled, bridge_services_in, ["carState", "deviceState"],
+  ))
+
+
@dispatcher.add_method
 def takeSnapshot() -> str | dict[str, str] | None:
  from openpilot.system.camerad.snapshot import jpeg_write, snapshot
@@ -26,6 +26,8 @@ public:
  virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0;
  virtual void encoder_open() = 0;
  virtual void encoder_close() = 0;
+  virtual void set_bitrate(int bitrate) = 0;
+  virtual void request_keyframe() = 0;

  void publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);

@@ -72,6 +72,14 @@ void FfmpegEncoder::encoder_close() {
  is_open = false;
 }

+void FfmpegEncoder::set_bitrate(int bitrate) {
+  LOGE("adaptive bitrate is not supported for ffmpeg encoder %s", encoder_info.publish_name);
+}
+
+void FfmpegEncoder::request_keyframe() {
+  LOGE("keyframe request is not supported for ffmpeg encoder %s", encoder_info.publish_name);
+}
+
 int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
  assert(buf->width == this->in_width);
  assert(buf->height == this->in_height);
@@ -21,6 +21,8 @@ public:
  int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
  void encoder_open();
  void encoder_close();
+  void set_bitrate(int bitrate);
+  void request_keyframe();

 private:
  int segment_num = -1;
@@ -156,6 +156,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei
  assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0);

  EncoderSettings encoder_settings = encoder_info.get_settings(in_width);
+  current_bitrate = encoder_settings.bitrate;
  bool is_h265 = encoder_settings.encode_type == cereal::EncodeIndex::Type::FULL_H_E_V_C;

  struct v4l2_format fmt_out = {
@@ -305,6 +306,36 @@ void V4LEncoder::encoder_close() {
  this->is_open = false;
 }

+void V4LEncoder::set_bitrate(int bitrate) {
+  if (bitrate == current_bitrate) return;
+  if (bitrate <= 0) {
+    LOGE("invalid livestream encoder bitrate %d", bitrate);
+    return;
+  }
+
+  struct v4l2_control ctrl = {
+    .id = V4L2_CID_MPEG_VIDEO_BITRATE,
+    .value = bitrate,
+  };
+
+  if (util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl) == -1) {
+    LOGE("failed to update %s bitrate to %d", encoder_info.publish_name, bitrate);
+    return;
+  }
+  current_bitrate = bitrate;
+}
+
+void V4LEncoder::request_keyframe() {
+  struct v4l2_control ctrl = {
+    .id = V4L2_CID_MPEG_VIDC_VIDEO_REQUEST_IFRAME,
+    .value = 1,
+  };
+
+  if (util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl) == -1) {
+    LOGE("failed to request keyframe for %s", encoder_info.publish_name);
+  }
+}
+
 V4LEncoder::~V4LEncoder() {
  encoder_close();
  v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
@@ -13,6 +13,8 @@ public:
  int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
  void encoder_open();
  void encoder_close();
+  void set_bitrate(int bitrate);
+  void request_keyframe();

 private:
  int fd;
@@ -20,6 +22,7 @@ private:
  bool is_open = false;
  int segment_num = -1;
  int counter = 0;
+  int current_bitrate = -1;

  SafeQueue<VisionIpcBufExtra> extras;

@@ -44,6 +44,19 @@ bool sync_encoders(EncoderdState *s, VisionStreamType cam_type, uint32_t frame_i
  }
 }

+void encoder_set_bitrate(std::unique_ptr<Encoder> &e) {
+  static Params params;
+  std::string val = params.get("LivestreamEncoderBitrate");
+  if (val.empty()) return;
+  e->set_bitrate(std::stoi(val));
+}
+
+void encoder_request_keyframe(std::unique_ptr<Encoder> &e) {
+  static Params params;
+  if (!params.getBool("LivestreamRequestKeyframe")) return;
+  e->request_keyframe();
+}
+

 void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
  util::set_thread_name(cam_info.thread_name);
@@ -110,6 +123,11 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {

      // encode a frame
      for (int i = 0; i < encoders.size(); ++i) {
+        if (cam_info.encoder_infos[i].is_live) {
+          encoder_set_bitrate(encoders[i]);
+          encoder_request_keyframe(encoders[i]);
+        }
+
        int out_id = encoders[i]->encode_frame(buf, &extra);

        if (out_id == -1) {
@@ -55,8 +55,8 @@ struct EncoderSettings {
  }

  static EncoderSettings StreamEncoderSettings() {
-    int _stream_bitrate = getenv("STREAM_BITRATE") ? atoi(getenv("STREAM_BITRATE")) : 1'000'000;
-    return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = _stream_bitrate , .gop_size = 15};
+    int _stream_bitrate = getenv("STREAM_BITRATE") ? atoi(getenv("STREAM_BITRATE")) : 5'000'000;
+    return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = _stream_bitrate , .gop_size = 5};
  }
 };

@@ -67,6 +67,7 @@ public:
  const char *filename = NULL;
  bool record = true;
  bool include_audio = false;
+  bool is_live = false;
  int frame_width = -1;
  int frame_height = -1;
  int fps = MAIN_FPS;
@@ -112,6 +113,7 @@ const EncoderInfo stream_road_encoder_info = {
  .publish_name = "livestreamRoadEncodeData",
  //.thumbnail_name = "thumbnail",
  .record = false,
+  .is_live = true,
  .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();},
  INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode),
 };
@@ -119,6 +121,7 @@ const EncoderInfo stream_road_encoder_info = {
 const EncoderInfo stream_wide_road_encoder_info = {
  .publish_name = "livestreamWideRoadEncodeData",
  .record = false,
+  .is_live = true,
  .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();},
  INIT_ENCODE_FUNCTIONS(LivestreamWideRoadEncode),
 };
@@ -126,6 +129,7 @@ const EncoderInfo stream_wide_road_encoder_info = {
 const EncoderInfo stream_driver_encoder_info = {
  .publish_name = "livestreamDriverEncodeData",
  .record = false,
+  .is_live = true,
  .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();},
  INIT_ENCODE_FUNCTIONS(LivestreamDriverEncode),
 };
@@ -62,12 +62,18 @@ def only_onroad(started: bool, params: Params, CP: car.CarParams, starpilot_togg
 def only_offroad(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
  return not started

+def livestream(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
+  return params.get_bool("IsLiveStreaming")
+
 def or_(*fns):
  return lambda *args: operator.or_(*(fn(*args) for fn in fns))

 def and_(*fns):
  return lambda *args: operator.and_(*(fn(*args) for fn in fns))

+def not_(*fns):
+  return lambda *args: operator.not_(*(fn(*args) for fn in fns))
+
 # StarPilot variables
 def allow_logging(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
  return not starpilot_toggles.no_logging
@@ -173,10 +179,10 @@ procs = [

  NativeProcess("loggerd", "system/loggerd", ["./loggerd"], and_(allow_logging, logging)),
  NativeProcess("encoderd", "system/loggerd", ["./encoderd"], and_(allow_logging, only_onroad)),
-  NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar),
+  NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], or_(and_(livestream, not_(iscar)), notcar)),
  PythonProcess("logmessaged", "system.logmessaged", always_run),

-  NativeProcess("camerad", "system/camerad", ["./camerad"], driverview, enabled=not WEBCAM),
+  NativeProcess("camerad", "system/camerad", ["./camerad"], or_(driverview, livestream), enabled=not WEBCAM),
  PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM),
  PythonProcess("proclogd", "system.proclogd", and_(allow_logging, only_onroad), enabled=platform.system() != "Darwin"),
  PythonProcess("journald", "system.journald", and_(allow_logging, only_onroad), platform.system() != "Darwin"),
@@ -217,7 +223,7 @@ procs = [

  # debug procs
  NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),
-  PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar),
+  PythonProcess("webrtcd", "system.webrtc.webrtcd", or_(and_(livestream, not_(iscar)), notcar)),
  PythonProcess("webjoystick", "tools.bodyteleop.web", notcar),
  PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)),
 ]
@@ -1,11 +1,22 @@
 import asyncio
+import struct
 import time

 import av
 from teleoprtc.tracks import TiciVideoStreamTrack
+from aiortc.mediastreams import MediaStreamError

 from cereal import messaging
-from openpilot.common.realtime import DT_MDL, DT_DMON
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+
+V4L2_BUF_FLAG_KEYFRAME = 0x8
+TIMING_SEI_UUID = bytes([
+  0xa5, 0xe0, 0xc4, 0xa4, 0x5b, 0x6e, 0x4e, 0x1e,
+  0x9c, 0x7e, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc,
+])
+_SEI_PREFIX = b'\x00\x00\x00\x01\x06\x05\x30' + TIMING_SEI_UUID


 class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
@@ -15,24 +26,64 @@ class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
    "road": "livestreamRoadEncodeData",
  }

-  def __init__(self, camera_type: str):
-    dt = DT_DMON if camera_type == "driver" else DT_MDL
-    super().__init__(camera_type, dt)
+  def __init__(self, camera_type: str, video_enabled: bool = True):
+    super().__init__(camera_type, DT_MDL)

-    self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True)
+    self._sock = self._make_sock(camera_type)
    self._pts = 0
    self._t0_ns = time.monotonic_ns()
+    self.timing_sei_enabled = False
+    self.params = Params()
+    self._seen_keyframe = False
+    self.video_enabled = video_enabled
+
+  def stop(self) -> None:
+    super().stop()
+    self._sock = None
+
+  def _make_sock(self, camera_type: str) -> messaging.SubSocket:
+    return messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True)
+
+  def switch_camera(self, camera_type: str) -> None:
+    self._sock = self._make_sock(camera_type)
+
+  def enable(self, enabled: bool):
+    self.video_enabled = enabled
+    if not enabled:
+      self._seen_keyframe = False
+
+  def _build_frame_data(self, msg) -> bytes:
+    encode_data = getattr(msg, msg.which())
+    if not self.timing_sei_enabled:
+      return encode_data.header + encode_data.data
+
+    idx = encode_data.idx
+    sei_nal = _SEI_PREFIX + struct.pack('>4d',
+      (idx.timestampEof - idx.timestampSof) / 1e6,
+      (msg.logMonoTime - idx.timestampEof) / 1e6,
+      (time.monotonic_ns() - msg.logMonoTime) / 1e6,
+      time.time() * 1000,  # noqa: TID251
+    ) + b'\x80'
+    return encode_data.header + sei_nal + encode_data.data

  async def recv(self):
    while True:
+      if self.readyState != "live":
+        raise MediaStreamError
+
+      if not self.video_enabled:
+        await asyncio.sleep(0.005)
+        continue
+
      msg = messaging.recv_one_or_none(self._sock)
      if msg is not None:
+        if not self._seen_keyframe and (getattr(msg, msg.which()).idx.flags & V4L2_BUF_FLAG_KEYFRAME):
+          self._seen_keyframe = True
+          self.params.put("LivestreamRequestKeyframe", False)
        break
      await asyncio.sleep(0.005)

-    evta = getattr(msg, msg.which())
-
-    packet = av.Packet(evta.header + evta.data)
+    packet = av.Packet(self._build_frame_data(msg))
    packet.time_base = self._time_base

    self._pts =  ((time.monotonic_ns() - self._t0_ns) * self._clock_rate) // 1_000_000_000
@@ -0,0 +1,41 @@
+import time
+import requests
+from dataclasses import asdict, dataclass, field
+
+
+WEBRTCD_PORT = 5001
+
+
+@dataclass
+class StreamRequestBody:
+  sdp: str
+  init_camera: str
+  enabled: bool
+  bridge_services_in: list[str] = field(default_factory=list)
+  bridge_services_out: list[str] = field(default_factory=list)
+
+
+def post_stream_request(body: StreamRequestBody) -> dict:
+  t_start = time.monotonic()
+  try:
+    resp = requests.post(f"http://localhost:{WEBRTCD_PORT}/stream", json=asdict(body), timeout=10)
+    t_end = time.monotonic()
+    ret = resp.json()
+    ret["time"] = (t_end - t_start) * 1000
+    return ret
+  except requests.ConnectTimeout as e:
+    raise Exception("webrtc took too long to respond.") from e
+  except requests.ConnectionError as e:
+    raise Exception("webrtc server on device is not running.") from e
+
+
+def wait_for_webrtcd(max_retries: float = 10) -> None:
+  attempts = 0
+  while attempts < max_retries:
+    try:
+      if requests.get(f"http://localhost:{WEBRTCD_PORT}/schema", timeout=1).ok:
+        return
+    except requests.ConnectionError:
+      attempts += 1
+      time.sleep(0.5)
+  raise TimeoutError("webrtcd did not initialize in time.")
@@ -39,7 +39,8 @@ class TestStreamSession:
      mocked_submaster.update_msgs(0, [test_msg])

    mocker.patch.object(messaging.SubMaster, "update", side_effect=mocked_update)
-    proxy = CerealOutgoingMessageProxy(mocked_submaster)
+    proxy = CerealOutgoingMessageProxy(["customReservedRawData0"])
+    proxy.sm = mocked_submaster
    proxy.add_channel(channel)

    proxy.update()
@@ -70,6 +71,7 @@ class TestStreamSession:

  def test_livestream_track(self, mocker):
    fake_msg = messaging.new_message("livestreamDriverEncodeData")
+    fake_msg.livestreamDriverEncodeData.idx.flags = 8

    config = {"receive.return_value": fake_msg.to_bytes()}
    mocker.patch("msgq.SubSocket", spec=True, **config)
@@ -31,7 +31,8 @@ class TestWebrtcdProc:
  async def test_webrtcd(self, mocker):
    mock_request = mocker.MagicMock()
    async def connect(offer):
-      body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': self.in_services, 'bridge_services_out': self.out_services}
+      body = {'sdp': offer.sdp, 'init_camera': offer.video[0], 'enabled': True,
+              'bridge_services_in': self.in_services, 'bridge_services_out': self.out_services}
      mock_request.json.side_effect = mocker.AsyncMock(return_value=body)
      response = await get_stream(mock_request)
      response_json = json.loads(response.text)
@@ -62,4 +63,3 @@ class TestWebrtcdProc:
    assert mock_request.app["streams"].__setitem__.called, "Implementation changed, please update this test"
    _, session = mock_request.app["streams"].__setitem__.call_args.args
    await self.assertCompletesWithTimeout(session.post_run_cleanup())
-
@@ -1,11 +1,15 @@
 #!/usr/bin/env python3

+from abc import abstractmethod
+import os
+import socket
+import time
 import argparse
 import asyncio
+import contextlib
 import json
 import uuid
 import logging
-from dataclasses import dataclass, field
 from typing import Any, TYPE_CHECKING

 # aiortc and its dependencies have lots of internal warnings :(
@@ -17,19 +21,77 @@ import capnp
 from aiohttp import web
 if TYPE_CHECKING:
  from aiortc.rtcdatachannel import RTCDataChannel
+import aioice.ice

+from openpilot.system.webrtc.helpers import StreamRequestBody
 from openpilot.system.webrtc.schema import generate_field
+from openpilot.common.params import Params
 from cereal import messaging, log


-class CerealOutgoingMessageProxy:
-  def __init__(self, sm: messaging.SubMaster):
-    self.sm = sm
+# socket trick: route lookup for 8.8.8.8 (nothing is sent or actually connected to)
+# return the source interfaces IP which is the default interface of the device
+def _default_route_ip() -> str | None:
+  s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+  try:
+    s.connect(("8.8.8.8", 53))  # selects a route, sends nothing
+    return s.getsockname()[0]
+  except OSError:
+    return None
+  finally:
+    s.close()
+
+# aioice patch: gather ICE candidates only on the default-route interface
+_get_host_addresses = aioice.ice.get_host_addresses
+def _primary_host_addresses(use_ipv4: bool, use_ipv6: bool) -> list[str]:
+  addresses = _get_host_addresses(use_ipv4, use_ipv6)
+  primary = _default_route_ip()
+  if primary not in addresses:
+    return addresses
+  return [primary, ]
+aioice.ice.get_host_addresses = _primary_host_addresses
+
+
+class AsyncTaskRunner:
+  def __init__(self):
+    self.is_running = False
+    self.task = None
+    self.logger = logging.getLogger("webrtcd")
+
+  def start(self):
+    assert self.task is None
+    self.task = asyncio.create_task(self.run())
+
+  async def stop(self):
+    if self.task is None:
+      return
+    task = self.task
+    self.task = None
+    if task.done():
+      return
+    task.cancel()
+    with contextlib.suppress(asyncio.CancelledError):
+      await task
+
+  @abstractmethod
+  async def run(self):
+    pass
+
+
+class CerealOutgoingMessageProxy(AsyncTaskRunner):
+  def __init__(self, services: list[str], enabled: bool = True):
+    super().__init__()
+    self.services = list(services)
+    self.sm = messaging.SubMaster(self.services)
    self.channels: list[RTCDataChannel] = []
+    self._enabled = enabled

  def add_channel(self, channel: 'RTCDataChannel'):
    self.channels.append(channel)

+  def enable(self, enable: bool):
+    self._enabled = enable
+
  def to_json(self, msg_content: Any):
    if isinstance(msg_content, capnp._DynamicStructReader):
      msg_dict = msg_content.to_dict()
@@ -55,6 +117,22 @@ class CerealOutgoingMessageProxy:
      for channel in self.channels:
        channel.send(encoded_msg)

+  async def run(self):
+    from aiortc.exceptions import InvalidStateError
+
+    while True:
+      if not self._enabled:
+        await asyncio.sleep(0.01)
+        continue
+      try:
+        self.update()
+      except InvalidStateError:
+        self.logger.warning("Cereal outgoing proxy invalid state (connection closed)")
+        break
+      except Exception:
+        self.logger.exception("Cereal outgoing proxy failure")
+      await asyncio.sleep(0.01)
+

 class CerealIncomingMessageProxy:
  def __init__(self, pm: messaging.PubMaster):
@@ -72,37 +150,6 @@ class CerealIncomingMessageProxy:
    self.pm.send(msg_type, msg)


-class CerealProxyRunner:
-  def __init__(self, proxy: CerealOutgoingMessageProxy):
-    self.proxy = proxy
-    self.is_running = False
-    self.task = None
-    self.logger = logging.getLogger("webrtcd")
-
-  def start(self):
-    assert self.task is None
-    self.task = asyncio.create_task(self.run())
-
-  def stop(self):
-    if self.task is None or self.task.done():
-      return
-    self.task.cancel()
-    self.task = None
-
-  async def run(self):
-    from aiortc.exceptions import InvalidStateError
-
-    while True:
-      try:
-        self.proxy.update()
-      except InvalidStateError:
-        self.logger.warning("Cereal outgoing proxy invalid state (connection closed)")
-        break
-      except Exception:
-        self.logger.exception("Cereal outgoing proxy failure")
-      await asyncio.sleep(0.01)
-
-
 class DynamicPubMaster(messaging.PubMaster):
  def __init__(self, *args, **kwargs):
    super().__init__(*args, **kwargs)
@@ -115,107 +162,235 @@ class DynamicPubMaster(messaging.PubMaster):
          self.sock[service] = messaging.pub_sock(service)


+class LivestreamBitrateController(AsyncTaskRunner):
+  bitrates = [500_000, 1_500_000, int(os.environ.get("STREAM_BITRATE", 5_000_000))]
+  label_to_bitrate = { "high": bitrates[2], "med": bitrates[1], "low": bitrates[0]}
+  sample_interval = 0.2
+  high_level = 0.1 # drop immediately
+  med_level = 0.05 # drop after # of samples
+  low_level = 0 # raise after # of samples
+  down_samples = 5 # 1s
+  param_name = "LivestreamEncoderBitrate"
+
+  def __init__(self, peer_connection: Any, params: Params, enabled: bool = True):
+    super().__init__()
+    self.pc = peer_connection
+    self.params = params
+
+    self.level = 2
+    self._publish(self.bitrates[self.level])
+    self.prev_lost, self.prev_sent = None, None
+    self.counter = 0
+    self.up_samples = 5 # 1s
+    self._auto = True
+    self._enabled = enabled
+
+  def enable(self, enable: bool):
+    self._enabled = enable
+
+  async def run(self):
+    while True:
+      await asyncio.sleep(self.sample_interval)
+      if not self._enabled:
+        continue
+      if not self._auto:
+        continue
+
+      loss_rate = await self._sample()
+      if loss_rate is None:
+        continue
+      if loss_rate >= self.med_level and self.level > 0:
+        self.counter += 1
+        if self.counter >= self.down_samples or loss_rate >= self.high_level:
+          self.level -= 1
+          self.up_samples *= 2 # exponential backoff before raising again
+          self.counter = 0
+          self._publish(self.bitrates[self.level])
+      elif loss_rate <= self.low_level and self.level < len(self.bitrates) - 1:
+        self.counter -= 1
+        if -self.counter >= self.up_samples:
+          self.level += 1
+          self.counter = 0
+          self._publish(self.bitrates[self.level])
+
+  async def _sample(self) -> float | None:
+    report = await self.pc.getStats()
+    packets_lost = packets_sent = 0
+    for s in report.values():
+      if s.type == "remote-inbound-rtp":
+        packets_lost += s.packetsLost
+      elif s.type == "outbound-rtp":
+        packets_sent += s.packetsSent
+
+    if self.prev_lost is None:
+      self.prev_lost, self.prev_sent = packets_lost, packets_sent
+      return None
+    lost_delta = max(0, packets_lost - self.prev_lost)
+    sent_delta = max(0, packets_sent - self.prev_sent)
+    self.prev_lost, self.prev_sent = packets_lost, packets_sent
+    return lost_delta / sent_delta if sent_delta else 0.0
+
+  def _publish(self, bitrate: float):
+    self.params.put(self.param_name, bitrate)
+
+  def set_quality(self, quality):
+    if quality in self.label_to_bitrate:
+      self._publish(self.label_to_bitrate[quality])
+      self._auto = False
+    elif quality == "auto":
+      self._auto = True
+
+
 class StreamSession:
  shared_pub_master = DynamicPubMaster([])

-  def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str], debug_mode: bool = False):
-    from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack
-    from aiortc.contrib.media import MediaBlackhole
+  def __init__(self, body: StreamRequestBody, debug_mode: bool = False):
+    if debug_mode:
+      from aiortc.mediastreams import AudioStreamTrack, VideoStreamTrack
+      from aiortc.contrib.media import MediaBlackhole
    from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack
    from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker
-    from teleoprtc import WebRTCAnswerBuilder
+    from teleoprtc.builder import WebRTCAnswerBuilder
    from teleoprtc.info import parse_info_from_offer

-    config = parse_info_from_offer(sdp)
-    builder = WebRTCAnswerBuilder(sdp)
+    self.identifier = str(uuid.uuid4())
+    self.params = Params()
+    builder = WebRTCAnswerBuilder(body.sdp)
+    config = parse_info_from_offer(body.sdp)

-    assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks"
-    for cam in cameras:
-      builder.add_video_stream(cam, LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack())
+    self.enabled = body.enabled
+    self.video_track = LiveStreamVideoStreamTrack(body.init_camera, self.enabled) if not debug_mode else VideoStreamTrack()
+    builder.add_video_stream(body.init_camera, self.video_track)
    if config.expected_audio_track:
      builder.add_audio_stream(AudioInputStreamTrack() if not debug_mode else AudioStreamTrack())
    if config.incoming_audio_track:
      self.audio_output_cls = AudioOutputSpeaker if not debug_mode else MediaBlackhole
      builder.offer_to_receive_audio_stream()
-
    self.stream = builder.stream()
-    self.identifier = str(uuid.uuid4())

    self.incoming_bridge: CerealIncomingMessageProxy | None = None
-    self.incoming_bridge_services = incoming_services
+    self.incoming_bridge_services = body.bridge_services_in
    self.outgoing_bridge: CerealOutgoingMessageProxy | None = None
-    self.outgoing_bridge_runner: CerealProxyRunner | None = None
-    if len(incoming_services) > 0:
-      self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master)
-    if len(outgoing_services) > 0:
-      self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services))
-      self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge)
-
+    self.bitrate_controller: LivestreamBitrateController | None = None
    self.audio_output: AudioOutputSpeaker | MediaBlackhole | None = None
+    if len(body.bridge_services_in) > 0:
+      self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master)
+    if len(body.bridge_services_out) > 0:
+      self.outgoing_bridge = CerealOutgoingMessageProxy(body.bridge_services_out, self.enabled)
+    self.bitrate_controller = LivestreamBitrateController(self.stream.peer_connection, self.params, self.enabled)
+
    self.run_task: asyncio.Task | None = None
+    self._cleanup_lock = asyncio.Lock()
+    self._cleanup_done = False
    self.logger = logging.getLogger("webrtcd")
-    self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s",
-                      self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services)
+    self.logger.info(
+      "New stream session (%s), init camera %s, video enabled %s, incoming services %s, outgoing services %s",
+      self.identifier, body.init_camera, body.enabled, body.bridge_services_in, body.bridge_services_out,
+    )

  def start(self):
    self.run_task = asyncio.create_task(self.run())

-  def stop(self):
-    if self.run_task.done():
-      return
-    self.run_task.cancel()
+  async def stop(self):
+    if self.run_task is not None and not self.run_task.done() and self.run_task is not asyncio.current_task():
+      self.run_task.cancel()
+      with contextlib.suppress(asyncio.CancelledError):
+        await self.run_task
    self.run_task = None
-    asyncio.run(self.post_run_cleanup())
+    await self.post_run_cleanup()

  async def get_answer(self):
    return await self.stream.start()

-  async def message_handler(self, message: bytes):
-    assert self.incoming_bridge is not None
+  def message_handler(self, message: bytes):
    try:
-      self.incoming_bridge.send(message)
+      payload = json.loads(message) if isinstance(message, (bytes, str)) else None
+      if isinstance(payload, dict):
+        msg_type = payload.get("type")
+
+        match msg_type:
+          case "livestreamCameraSwitch":
+            self.video_track.switch_camera(payload["data"]["camera"])
+          case "livestreamSettings":
+            self.bitrate_controller.set_quality(payload["data"]["quality"])
+          case "livestreamVideoEnable":
+            enabled = payload["data"]["enabled"]
+            self.enabled = enabled
+            self.video_track.enable(enabled)
+            if self.outgoing_bridge is not None:
+              self.outgoing_bridge.enable(enabled)
+            self.bitrate_controller.enable(enabled)
+            if not enabled:
+              self.params.put("LivestreamRequestKeyframe", True)
+          case "clockSync":
+            pong = json.dumps({"type": "clockSync", "data": {
+              "action": "pong", "browserSendTime": payload["data"]["browserSendTime"], "deviceTime": time.time() * 1000, # noqa: TID251
+            }})
+            self.stream.get_messaging_channel().send(pong)
+          case "enableTimingSei":
+            if hasattr(self.video_track, 'timing_sei_enabled'):
+              self.video_track.timing_sei_enabled = bool(payload["data"]["enabled"])
+          case _:
+            if payload.get("type") not in self.incoming_bridge_services:
+              return
+            self.incoming_bridge.send(message)
    except Exception:
      self.logger.exception("Cereal incoming proxy failure")

  async def run(self):
    try:
-      await self.stream.wait_for_connection()
+      self.params.put("LivestreamRequestKeyframe", True)
+      await asyncio.wait_for(self.stream.wait_for_connection(), timeout=15)
      if self.stream.has_messaging_channel():
+        self.stream.set_message_handler(self.message_handler)
        if self.incoming_bridge is not None:
          await self.shared_pub_master.add_services_if_needed(self.incoming_bridge_services)
-          self.stream.set_message_handler(self.message_handler)
-        if self.outgoing_bridge_runner is not None:
+        if self.outgoing_bridge is not None:
          channel = self.stream.get_messaging_channel()
-          self.outgoing_bridge_runner.proxy.add_channel(channel)
-          self.outgoing_bridge_runner.start()
+          self.outgoing_bridge.add_channel(channel)
+          self.outgoing_bridge.start()
      if self.stream.has_incoming_audio_track():
        track = self.stream.get_incoming_audio_track(buffered=False)
        self.audio_output = self.audio_output_cls()
        self.audio_output.addTrack(track)
        self.audio_output.start()
+      self.bitrate_controller.start()
+
      self.logger.info("Stream session (%s) connected", self.identifier)
-
      await self.stream.wait_for_disconnection()
-      await self.post_run_cleanup()
-
      self.logger.info("Stream session (%s) ended", self.identifier)
    except Exception:
      self.logger.exception("Stream session failure")
+    finally:
+      await self.post_run_cleanup()

  async def post_run_cleanup(self):
-    await self.stream.stop()
-    if self.outgoing_bridge is not None:
-      self.outgoing_bridge_runner.stop()
-    if self.audio_output:
-      self.audio_output.stop()
+    async with self._cleanup_lock:
+      if self._cleanup_done:
+        return
+      self._cleanup_done = True
+      self.params.put("LivestreamRequestKeyframe", False)
+      await self.bitrate_controller.stop()
+      if self.outgoing_bridge is not None:
+        await self.outgoing_bridge.stop()
+      if self.video_track is not None:
+        self.video_track.stop()
+        self.video_track = None
+      if self.audio_output is not None:
+        self.audio_output.stop()
+        self.audio_output = None
+      await self.stream.stop()


-@dataclass
-class StreamRequestBody:
-  sdp: str
-  cameras: list[str]
-  bridge_services_in: list[str] = field(default_factory=list)
-  bridge_services_out: list[str] = field(default_factory=list)
+def schedule_teardown(app):
+  # if nothing connects for 5 seconds, tear down livestreaming processes
+  h = app.get('teardown')
+  if h:
+      h.cancel()
+  def clear():
+      if not app['streams']:
+          Params().put_bool("IsLiveStreaming", False)
+  app['teardown'] = asyncio.get_running_loop().call_later(5.0, clear)


 async def get_stream(request: 'web.Request'):
@@ -223,22 +398,49 @@ async def get_stream(request: 'web.Request'):
  raw_body = await request.json()
  body = StreamRequestBody(**raw_body)

-  session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, debug_mode)
-  answer = await session.get_answer()
-  session.start()
+  async with request.app['stream_lock']:
+    # don't remove existing connection on prewarm request
+    enabled = any(s.run_task and not s.run_task.done() and s.enabled for s in stream_dict.values())
+    if enabled and not body.enabled:
+      return web.json_response({"error": "busy", "message": "someone else is connected."})

-  stream_dict[session.identifier] = session
+    for sid, s in list(stream_dict.items()):
+      if s.run_task and not s.run_task.done():
+        try:
+          ch = s.stream.get_messaging_channel()
+          ch.send(json.dumps({"type": "disconnect", "data": "Another device has connected, closing this session."}))
+        except Exception:
+          pass
+      await s.stop()
+      stream_dict.pop(sid, None)
+
+    session = StreamSession(body, debug_mode)
+    stream_dict[session.identifier] = session
+    try:
+      answer = await session.get_answer()
+    except Exception:
+      await session.stop()
+      stream_dict.pop(session.identifier, None)
+      logging.getLogger("webrtcd").exception("Failed to create stream answer")
+      raise
+    session.start()
+
+    def remove_finished_session(_: asyncio.Task) -> None:
+      stream_dict.pop(session.identifier, None)
+      schedule_teardown(request.app)
+    session.run_task.add_done_callback(remove_finished_session)

  return web.json_response({"sdp": answer.sdp, "type": answer.type})


 async def get_schema(request: 'web.Request'):
-  services = request.query["services"].split(",")
+  services = request.query.get("services", "").split(",")
  services = [s for s in services if s]
  assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name"
  schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services}
  return web.json_response(schema_dict)

+
 async def post_notify(request: 'web.Request'):
  try:
    payload = await request.json()
@@ -254,21 +456,48 @@ async def post_notify(request: 'web.Request'):

  return web.Response(status=200, text="OK")

+
 async def on_shutdown(app: 'web.Application'):
-  for session in app['streams'].values():
-    session.stop()
+  for session in list(app['streams'].values()):
+    try:
+      ch = session.stream.get_messaging_channel()
+      ch.send(json.dumps({"type": "disconnect", "data": "device streaming has been stopped."}))
+    except Exception:
+      pass
+    await session.stop()
  del app['streams']


+@web.middleware
+async def error_middleware(request: 'web.Request', handler):
+  try:
+    return await handler(request)
+  except Exception as e:
+    logging.getLogger("webrtcd").exception("Unhandled error handling %s", request.path)
+    return web.json_response({"error": "exception", "message": f"{type(e).__name__}: {e}"}, status=500)
+
+
+def prewarm_stream_session_imports(debug_mode: bool = False) -> None:
+  if debug_mode:
+    from aiortc.mediastreams import VideoStreamTrack
+    assert VideoStreamTrack
+  from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack
+  from teleoprtc.builder import WebRTCAnswerBuilder
+  assert LiveStreamVideoStreamTrack
+  assert WebRTCAnswerBuilder
+
+
 def webrtcd_thread(host: str, port: int, debug: bool):
  logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()])
-  logging_level = logging.DEBUG if debug else logging.INFO
-  logging.getLogger("WebRTCStream").setLevel(logging_level)
-  logging.getLogger("webrtcd").setLevel(logging_level)
+  prewarm_start = time.monotonic()
+  prewarm_stream_session_imports(debug)
+  prewarm_end = time.monotonic()
+  logging.getLogger("webrtcd").info(f"webrtc prewarm finished in {(prewarm_end - prewarm_start) * 1000} ms")

-  app = web.Application()
+  app = web.Application(middlewares=[error_middleware])

  app['streams'] = dict()
+  app['stream_lock'] = asyncio.Lock()
  app['debug'] = debug
  app.on_shutdown.append(on_shutdown)
  app.router.add_post("/stream", get_stream)
@@ -12,7 +12,7 @@ from aiohttp import web
 from aiohttp import ClientSession

 from openpilot.common.basedir import BASEDIR
-from openpilot.system.webrtc.webrtcd import StreamRequestBody
+from openpilot.system.webrtc.helpers import StreamRequestBody
 from openpilot.common.params import Params

 logger = logging.getLogger("bodyteleop")
@@ -96,7 +96,7 @@ async def sound(request: 'web.Request'):

 async def offer(request: 'web.Request'):
  params = await request.json()
-  body = StreamRequestBody(params["sdp"], ["driver"], ["testJoystick"], ["carState"])
+  body = StreamRequestBody(params["sdp"], "driver", True, ["testJoystick"], ["carState"])
  body_json = json.dumps(dataclasses.asdict(body))

  logger.info("Sending offer to webrtcd...")