-
Notifications
You must be signed in to change notification settings - Fork 9.2k
Ford LateralMotionControl
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.
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).
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.
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.
- Analysis of stock route by jyoung8607: //github.com/commaai/openpilot/pull/23331#issuecomment-1015277920
- Jason came to the conclusion that we should use curvature/curvature rate, although I think this would work fine for the highway, I don't think it would be able to handle turns
The stock camera always sends the lane centering command, even when the system isn't engaged, except the active bit is set low.
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 |
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 |
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 |
| 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 |
| 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 |
# 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";
Make driving chill. Buy a comma 3X with a car harness today!