From 92d21da55219bb51797a1a90a40307b24b38d23d Mon Sep 17 00:00:00 2001
From: Cameron Clough <cameronjclough@gmail.com>
Date: Mon, 25 Jul 2022 21:02:27 +0100
Subject: [PATCH] Ford: fix gear parsing (#25267)

* Ford: fix gear shifter state

* Ford: manumatic is drive

* add full gear names to CarStateBase

* fix for unrecognised gears

* Update interfaces.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 2351ae48b932eb7942178954811695b1a98e66e1
---
 selfdrive/car/ford/carstate.py  | 14 ++------------
 selfdrive/car/ford/interface.py |  4 ++--
 selfdrive/car/ford/values.py    |  1 +
 selfdrive/car/interfaces.py     | 21 +++++++++++++++------
 4 files changed, 20 insertions(+), 20 deletions(-)

diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index 67a72fd67c..aaa3af612f 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -1,5 +1,3 @@
-from typing import Dict
-
 from cereal import car
 from common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
@@ -51,8 +49,8 @@ class CarState(CarStateBase):
 
     # gear
     if self.CP.transmissionType == TransmissionType.automatic:
-      gear = int(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"])
-      ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
+      gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"], None)
+      ret.gearShifter = self.parse_gear_shifter(gear)
     elif self.CP.transmissionType == TransmissionType.manual:
       ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
       if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
@@ -85,14 +83,6 @@ class CarState(CarStateBase):
 
     return ret
 
-  @staticmethod
-  def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
-    d: Dict[str, car.CarState.GearShifter] = {
-        'Park': GearShifter.park, 'Reverse': GearShifter.reverse, 'Neutral': GearShifter.neutral,
-        'Manual': GearShifter.manumatic, 'Drive': GearShifter.drive,
-    }
-    return d.get(gear, GearShifter.unknown)
-
   @staticmethod
   def get_can_parser(CP):
     signals = [
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index 857ccbab8b..405579fa60 100644
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -2,7 +2,7 @@
 from cereal import car
 from common.conversions import Conversions as CV
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
-from selfdrive.car.ford.values import TransmissionType, CAR
+from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter
 from selfdrive.car.interfaces import CarInterfaceBase
 
 
@@ -70,7 +70,7 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
 
-    events = self.create_common_events(ret)
+    events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
     ret.events = events.to_msg()
 
     return ret
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index aae011ad3d..6b8396cf07 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -7,6 +7,7 @@ from selfdrive.car.docs_definitions import CarInfo
 
 Ecu = car.CarParams.Ecu
 TransmissionType = car.CarParams.TransmissionType
+GearShifter = car.CarState.GearShifter
 
 AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
 
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index a451ef2ae1..38a613b0e7 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -2,7 +2,7 @@ import yaml
 import os
 import time
 from abc import abstractmethod, ABC
-from typing import Any, Dict, Tuple, List
+from typing import Any, Dict, Optional, Tuple, List
 
 from cereal import car
 from common.basedir import BASEDIR
@@ -318,13 +318,22 @@ class CarStateBase(ABC):
     return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
 
   @staticmethod
-  def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
+  def parse_gear_shifter(gear: Optional[str]) -> car.CarState.GearShifter:
+    if gear is None:
+      return GearShifter.unknown
+    
     d: Dict[str, car.CarState.GearShifter] = {
-        'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
-        'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive,
-        'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake
+        'P': GearShifter.park, 'PARK': GearShifter.park,
+        'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse,
+        'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral,
+        'E': GearShifter.eco, 'ECO': GearShifter.eco,
+        'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic,
+        'D': GearShifter.drive, 'DRIVE': GearShifter.drive,
+        'S': GearShifter.sport, 'SPORT': GearShifter.sport,
+        'L': GearShifter.low, 'LOW': GearShifter.low,
+        'B': GearShifter.brake, 'BRAKE': GearShifter.brake,
     }
-    return d.get(gear, GearShifter.unknown)
+    return d.get(gear.upper(), GearShifter.unknown)
 
   @staticmethod
   def get_cam_can_parser(CP):