From 2ce908d756bfcb6f24ed17e431c152c329be5440 Mon Sep 17 00:00:00 2001 From: Simon Kuang Date: Sat, 28 Oct 2023 11:29:09 -0700 Subject: [PATCH] sin(roll)cos(pitch) feedforward in paramsd. params.roll is now reinterpreted as "gravity factor" rather than physical configuration, but this reinterpretation agrees perfectly with the existing linear model of acc = "roll" * g https://github.com/commaai/openpilot/issues/30341 --- selfdrive/locationd/paramsd.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 0e779c9e3e596c4..3421de5a173f4f9 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -52,8 +52,24 @@ def handle_log(self, t, which, msg): self.yaw_rate = msg.angularVelocityCalibrated.value[2] self.yaw_rate_std = msg.angularVelocityCalibrated.std[2] - localizer_roll = msg.orientationNED.value[0] - localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] + # The "roll" param should be renamed to "gravity factor." + # It is the scalar projection of the ground normal onto the car's lateral axis. + # TODO(simontheflutist) use a dynamic model to account for speed effect. + # At high longitudinal speeds, the car is sort of in free fall (hence the simple calculation). + # But at lower speeds, there is a lot of lateral friction. + llk_roll = msg.orientationNED.value[0] + llk_roll_std = msg.orientationNED.std[0] + llk_pitch = msg.orientationNED.value[1] + llk_pitch_std = msg.orientationNED.std[1] + localizer_roll = math.cos(llk_pitch) * math.sin(llk_roll) + if np.isnan(msg.orientationNED.std[0]) or np.isnan(msg.orientationNED.std[0]): + localizer_roll_std = np.radians(1) + else: + # from linearization of localizer_roll as a function of roll and pitch + localizer_roll_std = ( + (math.cos(llk_pitch) * math.cos(llk_roll) * llk_roll_std)**2 + + (math.sin(llk_pitch) * math.sin(llk_roll) * llk_pitch_std)**2 + )**0.5 self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK if self.roll_valid: roll = localizer_roll