Skip to content

Ford LateralMotionControl

Alexander Ose edited this page Apr 6, 2024 · 49 revisions

◄ Ford

Notes

The Ford "Lane Centering" feature uses the LateralMotionControl message to control steering.

The Lane Centering feature is not the same as Lane Keep Assist, which had the 10 second "lockout". The Lane Keep Assist command is still present on almost all Ford vehicles and is now branded as Co-Pilot 360. Lane Centering is usually packaged with Co-Pilot 360 Assist+ or Assist 2.0.

values = {
  "LatCtl_D_Rq": lca_rq,                      # Mode: 0=NoLateralControl, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight [0|7]
  "LatCtlRampType_D_Rq": ramp_type,           # Ramp type: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
  "LatCtlPrecision_D_Rq": precision,          # Precision: 0=Comfortable, 1=Precise [0|3]
  "LatCtlPathOffst_L_Actl": path_offset,      # Path offset [-5.12|5.11] meter
  "LatCtlPath_An_Actl": path_angle,           # Path angle [-0.5|0.5235] radians
  "LatCtlCurv_No_Actl": curvature,            # Curvature [-0.02|0.02094] 1/meter
  "LatCtlCurv_NoRate_Actl": curvature_rate,   # Curvature rate [-0.001024|0.00102375] 1/meter^2
}

jyoung8607 commented on Jan 17

The LateralMotionControl command carries a path angle (as opposed to steering angle), an offset from the center of that path, a curvature and curvature rate, and how aggressive it should be about following it. The implication is that your IPMA sees a path it wants to drive, describes that path to the the PSCM, and otherwise says "pls just handle it kthxbai".

The power steering module (PSCM) presumably combines this information with other data (speed, yaw rate) to determine the correct steering angle.

Experiments

I did experiments sending curvature alone (see this comment) and from memory this worked okay on the highway, I don't have any logs for this though since it was from early this year. I think the problem with using only the curvature signal is that the signal range is small (-0.02 to 0.02 m^-1) so in theory it could only navigate curves with a radius > 50 metres. EDIT: I think this assumption was wrong. It seems to perform well on highway curves.

I also tried sending curvature and path offset and previously opened a PR, but it was clear that the way I set it up to take information from the model and lateral plan wasn't the right way to do it - we need to calculate the signals from the desired_curvature instead.

I haven't experimented with the path offset signal yet, and although I know that this does actuate steering I have no idea how the steering angle is calculated from this (the precision and ramp type signals probably control this).

Path angle

I've also experimented with using the path angle signal, and this is what the users on my fork are driving on now. We can send values from -0.5 to 0.5 radians and the steering angle seems to be proportional to this, but looking at the logs there is a weird ratio (x2.75) between the command that we send and the actual steering angle.

command: 0.5 radians => steering angle: 80 degrees

path_angle = math.radians(actuators.steeringAngleDeg) / CarControllerParams.LKAS_RATIO  # 2.75

However, when testing with a joystick in a car park I found that by slowly ramping up the command I could get the wheel to steer beyond 180 degrees, so there is something weird going on. I don't think it is only related to speed because I checked and the ratio in the logs is the same at 30mph as 70mph. My script for calculating this could be wrong, but I am using the 2.75 ratio on my fork and users complained when I decreased the ratio for 30mph that it was steering too much. It might be that we have to be smart about how quickly the command signal is ramped up.

Also, the EPS can sometimes freeze up when the signal is ramped up too quickly. I don't know if this a limit on the signal, or something to do with the output torque. There is no signal for the EPS motor torque.

Hugging / Roll Compensation

Since the PSCM does extra calculations on the signals we send, many users experience hugging as the car doesn't achieve the desired steering angle.

I have tried using an integral term to learn the offset between the desired and actual steering angle and correct for it, but it doesn't seem to have helped. I haven't been able to iterate much on this since I started testing it after we returned the last car.

Useful links

Routes

The stock camera always sends the lane centering command, even when the system isn't engaged, except the active bit is set low.

Bronco Sport

car we rented

Route connect code Notes
62241b0c7fea4589|2022-10-25--21-39-41 connect openpilot using LCA curvature signal felt pretty good, have not reviewed data yet
62241b0c7fea4589|2022-10-25--15-00-46 connect stock ACC and LCA did not perform very well
62241b0c7fea4589|2022-10-25--11-50-48 connect NO ADAS car we rented

Explorer

the car we rented

Route connect code Notes
62241b0c7fea4589|2022-09-01--15-32-49 connect ford-soon branch the car we rented
62241b0c7fea4589|2022-08-30--13-28-11 connect stock the car we rented

Explorer

other explorers

Route connect code Notes
59a107f5793d9cc0|2022-10-23--12-33-09 connect ford-devel
59a107f5793d9cc0|2022-10-08--16-03-20 connect ford-devel branch trying breakpoints for path angle ratio, did not get good feedback

Escape

Route connect code Notes
f8eaaccd2a90aef8|2022-10-24--19-24-24--0 connect stock
f8eaaccd2a90aef8|2022-10-22--13-39-26--0 connect ford-devel branch using 2.75 path angle ratio and 0.01 integral term
26b2cace68e36212|2022-10-21--23-53-26 connect ford-devel branch using 2.75 path angle ratio and 0.01 integral term

Focus

Route connect code Notes
86d00e12925f4df7|2022-08-15--16-12-59 connect ad555a2 path_angle = math.radians(actuators.steeringAngleDeg) * 4.1 / self.VM.sR
86d00e12925f4df7|2022-08-12--17-12-18 connect fe8617e path_angle = math.radians(actuators.steeringAngleDeg) * 4.1 / self.VM.sR
86d00e12925f4df7|2022-08-05--15-28-15 connect ba0afe7 same as 65fcfa7 but rebased path_angle = math.radians(actuators.steeringAngleDeg) * 4.15 / self.VM.sR
86d00e12925f4df7|2022-08-01--20-02-49 connect 65fcfa7 path_angle = math.radians(actuators.steeringAngleDeg) * 4.15 / self.VM.sR
86d00e12925f4df7|2022-07-28--16-19-12 connect 41c7334 path_angle = math.radians(actuators.steeringAngleDeg) * 4. / self.VM.sR
86d00e12925f4df7|2022-07-19--15-02-16 connect 42e9333 path_angle = math.radians(actuators.steeringAngleDeg) * 4.52182 / self.VM.sR
86d00e12925f4df7|2022-06-18--11-45-12 connect b8c9893 path_angle = math.radians(actuators.steeringAngleDeg) * 0.285

DBC

ford_licoln_base_pt.dbc

# Lane Keep Assist message
BO_ 970 Lane_Assist_Data1: 8 IPMA_ADAS
 SG_ LkaDrvOvrrd_D_Rq : 38|2@0+ (1,0) [0|3] "SED"  PSCM
 SG_ LkaActvStats_D2_Req : 7|3@0+ (1,0) [0|7] "SED"  PSCM
 SG_ LaRefAng_No_Req : 19|12@0+ (0.05,-102.4) [-102.4|102.3] "mrad"  PSCM
 SG_ LaRampType_B_Req : 39|1@0+ (1,0) [0|1] "SED"  PSCM
 SG_ LaCurvature_No_Calc : 15|12@0+ (5E-006,-0.01024) [-0.01024|0.01023] "1/m"  PSCM
 SG_ LdwActvStats_D_Req : 4|3@0+ (1,0) [0|7] "SED"  PSCM
 SG_ LdwActvIntns_D_Req : 1|2@0+ (1,0) [0|3] "SED"  PSCM

# Lane Centering message (not present on CAN FD platform)
BO_ 979 LateralMotionControl: 8 IPMA_ADAS
 SG_ LatCtlRng_L_Max : 63|6@0+ (2,0) [0|126] "meter"  PSCM
 SG_ HandsOffCnfm_B_Rq : 51|1@0+ (1,0) [0|1] "SED"  PSCM
 SG_ LatCtl_D_Rq : 36|3@0+ (1,0) [0|7] "SED"  PSCM
 SG_ LatCtlRampType_D_Rq : 53|2@0+ (1,0) [0|3] "SED"  PSCM
 SG_ LatCtlPrecision_D_Rq : 33|2@0+ (1,0) [0|3] "SED"  PSCM
 SG_ LatCtlPathOffst_L_Actl : 47|10@0+ (0.01,-5.12) [-5.12|5.11] "meter"  PSCM
 SG_ LatCtlPath_An_Actl : 31|11@0+ (0.0005,-0.5) [-0.5|0.5235] "radians"  PSCM
 SG_ LatCtlCurv_NoRate_Actl : 12|13@0+ (2.5E-007,-0.001024) [-0.001024|0.00102375] "1/meter"  PSCM
 SG_ LatCtlCurv_No_Actl : 7|11@0+ (2E-005,-0.02) [-0.02|0.02094] "1/meter"  PSCM

# alternative Lane Centering message (not present on CAN platform)
BO_ 982 LateralMotionControl2: 8 IPMA_ADAS
 SG_ LatCtlCrv_NoRate2_Actl : 55|11@0+ (1E-006,-0.001024) [-0.001024|0.001023] "meter^2"  PSCM
 SG_ LatCtlPath_No_Cnt : 60|4@0+ (1,0) [0|15] "Unitless"  PSCM
 SG_ LatCtlPath_No_Cs : 15|8@0+ (1,0) [0|255] "Unitless"  PSCM
 SG_ LatCtl_D2_Rq : 6|3@0+ (1,0) [0|7] "SED"  PSCM
 SG_ HandsOffCnfm_B_Rq : 7|1@0+ (1,0) [0|1] "SED"  PSCM
 SG_ LatCtlRampType_D_Rq : 1|2@0+ (1,0) [0|3] "SED"  PSCM
 SG_ LatCtlPrecision_D_Rq : 3|2@0+ (1,0) [0|3] "SED"  PSCM
 SG_ LatCtlPathOffst_L_Actl : 33|10@0+ (0.01,-5.12) [-5.12|5.11] "meter"  PSCM
 SG_ LatCtlPath_An_Actl : 28|11@0+ (0.0005,-0.5) [-0.5|0.5235] "radians"  PSCM
 SG_ LatCtlCurv_No_Actl : 23|11@0+ (2E-005,-0.02) [-0.02|0.02094] "1/meter"  PSCM


VAL_ 970 LkaDrvOvrrd_D_Rq 3 "Level_3" 2 "Level_2" 1 "Level_1" 0 "Level_0";
VAL_ 970 LkaActvStats_D2_Req 7 "NotUsed" 6 "LkaIncrIntervRight" 5 "LkaSupprRight" 4 "LkaStandIntervRight" 3 "LkaSupprLeft" 2 "LkaStandIntervLeft" 1 "LkaIncrIntervLeft" 0 "LkaNoInterv";
VAL_ 970 LaRefAng_No_Req 4095 "Fault";
VAL_ 970 LaRampType_B_Req 1 "Quick" 0 "Smooth";
VAL_ 970 LaCurvature_No_Calc 4095 "Fault";
VAL_ 970 LdwActvStats_D_Req 7 "LDW_Suppress_Right_Left" 6 "Not_Used2" 5 "LDW_Suppress_Right" 4 "LDW_Warning_Right" 3 "LDW_Suppress_Left" 2 "LDW_Warning_Left" 1 "LDW_DemoVibration" 0 "LDW_Idle";
VAL_ 970 LdwActvIntns_D_Req 3 "High" 2 "Medium" 1 "Low" 0 "None";

VAL_ 979 HandsOffCnfm_B_Rq 1 "Active" 0 "Inactive";
VAL_ 979 LatCtl_D_Rq 7 "NotUsed4" 6 "NotUsed3" 5 "NotUsed2" 4 "NotUsed1" 3 "InterventionRight" 2 "InterventionLeft" 1 "ContinuousPathFollowing" 0 "NoLateralControl";
VAL_ 979 LatCtlRampType_D_Rq 3 "Immediately" 2 "Fast" 1 "Medium" 0 "Slow";
VAL_ 979 LatCtlPrecision_D_Rq 3 "NotUsed2" 2 "NotUsed1" 1 "Precise" 0 "Comfortable";

VAL_ 982 LatCtl_D2_Rq 7 "NotUsed_5" 6 "NotUsed_4" 5 "NotUsed_3" 4 "NotUsed_2" 3 "SafeRampOut" 2 "PathFollowingExtendedMode" 1 "PathFollowingLimitedMode" 0 "NoLateralControl";
VAL_ 982 HandsOffCnfm_B_Rq 1 "Active" 0 "Inactive";
VAL_ 982 LatCtlRampType_D_Rq 3 "Immediately" 2 "Fast" 1 "Medium" 0 "Slow";
VAL_ 982 LatCtlPrecision_D_Rq 3 "NotUsed2" 2 "NotUsed1" 1 "Precise" 0 "Comfortable";
Clone this wiki locally