This post will walk through how OpenPilot plans in the longitudinal direction.

We can directly look into how the planner_thread works:

We can see the longitudinal plan is conducted by a ** Planner** object. The key step for the longitudinal planner is:

*PL.update.*Let’s dive into the planner object:

In the planner object, the key function is ** update(), which outputs the plan message directly. Notice that the planner with and without model are different. **Now let’s break down the update function:

If the planner is using the neural network model as one of the solution sources, the update (every planning step) should look like this:

First, we check out the official implementation from Openpilot:

The idea is to create 2 regular mpcs, and 1 mpc_model which takes the NN model outputs (speed, distance, and acceleration vectors) as the input and solves the optimal next-step speed.

From OP community, we also see different implementations of the e2e brake. Notice that model is only used for braking because the choose_solution is to find the lowest planning speed. This means for the acceleration case, we don't have e2e acceleration yet.

The logic is that, if the dp_slow_on_curve is activated, we use the model speed. (Model is able to output a lower speed on curve)

`if self.dp_slow_on_curve: `

solutions = {'model': self.v_model, 'cruise': self.v_cruise}

Impact of the curve:

a speed smoother on the curve

`if self.dp_slow_on_curve: `

**self.v_model, self.a_model** = *speed_smoother*(self.v_acc_start, self.a_acc_start,model_speed, 2*accel_limits[1], accel_limits[0], 2*jerk_limits[1], jerk_limits[0], LON_MPC_STEP)

First, it gets the ** cruise_speed** in kph units.

v_ego = sm['carState'].vEgolong_control_state = sm['controlsState'].longControlStatev_cruise_kph = sm['controlsState'].vCruiseforce_slow_decel = sm['controlsState'].forceDecelv_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS

Then it gets the lead vehicle information from radarState:

lead_1 = sm['radarState'].leadOne

lead_2 = sm['radarState'].leadTwoenabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)following= lead_1.status andlead_1.dRel < 45.0 and lead_1.vLeadK > v_egoand lead_1.aLeadK > 0.0

Note that the following state requires some conditions. Obviously, the conditions are related to the relevant distance and speed from the lead_1 vehicle.

**lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego**

So, does it mean the vehicle is not following if the relevant distance is larger than 45 meters?

Now we calculate the speed for normal cruise control:

It calculates the limits for accelerations, jerks, and accelerations in turns. Then it applies a ** speed smoother** to get

**and**

*v_cruse***.**

*a_cruise*Below is how the two mpc controller works?

The two mpc objects set their current state using (**v_acc_start, a_acc_start**) and then update the pm and sm messages.

What are the *v_acc_start and a_acc_start? What are their roles for mpc controllers?*

self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)

self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)self.mpc1.update(pm, sm['carState'], lead_1)self.choose_solution(v_cruise_setpoint, enabled)

self.mpc2.update(pm, sm['carState'], lead_2)

Finally we apply the choose_solution function on the results from all the available mpc controllers.

`self.choose_solution(v_cruise_setpoint, enabled)`

Now let’s look into how mpc1 and mpc2 set their current state and update the planning results?

- when initializing the two mpc controllers, the mpc class does the following:

self.setup_mpc()

self.v_mpc = 0.0

self.v_mpc_future = 0.0

self.a_mpc = 0.0

self.v_cruise = 0.0

self.prev_lead_status = False

self.prev_lead_x = 0.0

self.new_lead = Falseself.last_cloudlog_t = 0.0ffi, self.libmpc =def setup_mpc(self):(self.mpc_id)libmpc_py.get_libmpc

self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,

MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.mpc_solution = ffi.new("log_t *")

self.cur_state = ffi.new("state_t *")

self.cur_state[0].v_ego = 0

self.cur_state[0].a_ego = 0

self.a_lead_tau = _LEAD_ACCEL_TAU

Most importantly, it instantiates a mpc solver written in C++.

2. to set the current state, it only takes in the current ego_v and ego_a:

`def set_cur_state(self, v, a):`

self.cur_state[0].v_ego = v

self.cur_state[0].a_ego = a

3. Then the mpc controller executes the update(pm,CS,lead) step. Here it takes the input of the Pub message, current car state, as well as the **lead** information.

The ** pm** is to receive the results. The

**information is the most important input.**

*lead*The mpc controller first set up its current state:

It uses the ** lead** information to get the x_lead, v_lead and a_lead.

We see mpc controller initialize the simulation with:

`self.libmpc.`**init_with_simulation**(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)

Then it calculates mpc:

`# Calculate mpc`

t = sec_since_boot()

n_its = **self.libmpc.run_mpc**(self.cur_state, self.mpc_solution, **self.a_lead_tau, a_lead**)

The regular mpc only takes the lead vehicle information.

Note that the regular mpc controllers are different from the model_mpc.

The model_mpc takes the input of the predicted pose, v and a

def update(self, v_ego, a_ego,poss, speeds, accels):

if len(poss) == 0:

self.valid = False

return x_poly = list(map(float, np.polyfit(self.ts, poss, 3)))

v_poly = list(map(float, np.polyfit(self.ts, speeds, 3)))

a_poly = list(map(float, np.polyfit(self.ts, accels, 3))) # Calculate mpc

self.libmpc.run_mpc(self.cur_state, self.mpc_solution,x_poly, v_poly, a_poly)

The details of the MPC controller for longitudinal control. We look into the c file *longtudinal_mpc.c:*

Notice that in the c file, it defines **N** (Number of intervals in the horizon), and the other two types of struct, **state_t, and log_t.** The state_t corresponds to the current state of ego and lead vehicle. The log_t includes the ref trajectory of the ego vehicle and the lead vehicle and a **cost.**

#defineNACADO_N /*Number of intervals in the horizon. */notice N = 20ACADOvariables acadoVariables;

ACADOworkspace acadoWorkspace;typedef struct {

double x_ego, v_ego, a_ego, x_l, v_l, a_l;

}state_t;typedef struct {

double x_ego[N+1];// 66, a list

double v_ego[N+1];//66, should be the ref traj

double a_ego[N+1];

double j_ego[N];

double x_l[N+1];

double v_l[N+1];

double a_l[N+1];

double t[N+1];

double cost;

}log_t;

Notice that the ** run_mpc** takes state_t and log_t struct as the input arguments. It requires that the self.cur_state is a state_t struct, and the self.mpc_solution is a log_t struct.

int run_mpc(state_t* x0,log_t* solution, double l, double a_l_0)# it was called:

n_its =self.libmpc.run_mpc(self.cur_state, self.mpc_solution,self.a_lead_tau, a_lead)

4. We see there are two mpc controllers based on lead1 and lead2. What matters from an mpc controller is the speed, which is self.mpc1.v_mpc

OP actually compares the v_mpc from different controllers, and executes the slowest speed from all controller sources.

def choose_solution(self, v_cruise_setpoint, enabled):

if enabled:

solutions = {'cruise': self.v_cruise}

if self.mpc1.prev_lead_status:

solutions['mpc1'] =self.mpc1.v_mpc

if self.mpc2.prev_lead_status:

solutions['mpc2'] =self.mpc2.v_mpcslowest = min(solutions, key=solutions.get)self.longitudinalPlanSource = slowest

## we can add model predicted speed here

# Choose lowest of MPC and cruise

if slowest == 'mpc1':

self.v_acc = self.mpc1.v_mpc

self.a_acc = self.mpc1.a_mpc

elif slowest == 'mpc2':

self.v_acc = self.mpc2.v_mpc

self.a_acc = self.mpc2.a_mpc

elif slowest == 'cruise':

self.v_acc = self.v_cruise

self.a_acc = self.a_cruiseself.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])

The planner that incorporates model: