diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 41ce2fd8a0..0949ffe226 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,3 +1,4 @@ +import math from cereal import car from selfdrive.car import make_can_msg from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button @@ -33,7 +34,7 @@ class CarController(): if (frame % 3) == 0: - curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*3.1415/180., CS.out.vEgo) + curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 8fe7c74d95..604851cb9d 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import math from cereal import car from selfdrive.config import Conversions as CV from selfdrive.swaglog import cloudlog @@ -10,7 +11,7 @@ from selfdrive.car.interfaces import CarInterfaceBase TS = 0.01 # 100Hz YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter # low pass gain -LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS) +LPG = 2 * math.pi * YAW_FR * TS / (1 + 2 * math.pi * YAW_FR * TS) class CarInterface(CarInterfaceBase): diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 96488f4cc4..ca4be4c255 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -1,8 +1,8 @@ +#include #include #include "common/modeldata.h" -#define PI 3.1415926536 -#define deg2rad(d) (d/180.0*PI) +#define deg2rad(d) (d/180.0*M_PI) const int N_steps = 16; using namespace std; @@ -23,7 +23,7 @@ int main( ) OnlineData rotation_radius; Control curvature_rate; - + // Equations of motion f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * curvature);