diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 0e779c9e3e596c..3421de5a173f4f 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