Common & Control : minior cleaning up

modules/common/configs/proto/vehicle_config.proto

@@ -59,6 +59,9 @@ enum VehicleBrand {
   // Tire effective rolling radius (vertical distance between the wheel center
   // and the ground).
   optional double wheel_rolling_radius = 17;
+
+  // minimum differentiable vehicle speed, in m/s
+  optional float max_abs_speed_when_stopped = 18;
 }
 
 message VehicleConfig {

modules/common/data/gem_config.pb.txt

@@ -16,4 +16,5 @@ vehicle_param {
   steer_ratio: 23
   wheel_base: 2.565
   wheel_rolling_radius: 0.275
+  max_abs_speed_when_stopped: 0.15
 }

modules/common/data/mkz_config.pb.txt

@@ -16,4 +16,5 @@ vehicle_param {
   steer_ratio: 16
   wheel_base: 2.8448
   wheel_rolling_radius: 0.335
+  max_abs_speed_when_stopped: 0.2
 }

modules/control/common/control_gflags.cc

@@ -44,10 +44,7 @@ DEFINE_int32(max_planning_miss_num, 20,
 
 DEFINE_double(max_acceleration_when_stopped, 0.01,
               "max acceleration can be observed when vehicle is stopped");
-DEFINE_double(
-    max_abs_speed_when_stopped, 0.25,
-    "max absolute speed can be observed when vehicle is stopped, will reconfig "
-    "it in different vehicles since this is chassis minimum speed feedback");
+
 DEFINE_double(steer_angle_rate, 100.0,
               "Steer angle change rate in percentage.");
 DEFINE_bool(enable_gain_scheduler, false,

modules/control/common/control_gflags.h

@@ -42,7 +42,6 @@ DECLARE_int32(max_chassis_miss_num);
 DECLARE_int32(max_planning_miss_num);
 
 DECLARE_double(max_acceleration_when_stopped);
-DECLARE_double(max_abs_speed_when_stopped);
 
 DECLARE_double(steer_angle_rate);
 DECLARE_bool(enable_gain_scheduler);

modules/control/conf/lincoln.pb.txt

@@ -12,6 +12,7 @@ active_controllers: LON_CONTROLLER
 max_steering_percentage_allowed: 100
 minimum_speed_resolution: 0.2
 query_relative_time: 0.8
+minimum_speed_protection: 0.1
 lat_controller_conf {
   ts: 0.01
   preview_window: 0

modules/control/conf/navigation_lincoln.pb.txt

@@ -12,6 +12,7 @@ active_controllers: LON_CONTROLLER
 max_steering_percentage_allowed: 100
 minimum_speed_resolution: 0.05
 query_relative_time: 0.2
+minimum_speed_protection: 0.1
 lat_controller_conf {
   ts: 0.01
   preview_window: 0

modules/control/controller/BUILD

@@ -51,6 +51,7 @@ cc_library(
     deps = [
         ":controller_interface",
         "//modules/common:log",
+        "//modules/common/configs:vehicle_config_helper",
         "//modules/common/filters:digital_filter",
         "//modules/common/filters:digital_filter_coefficients",
         "//modules/common/status",

modules/control/controller/lat_controller.cc

@@ -124,6 +124,8 @@ bool LatController::LoadControlConf(const ControlConf *control_conf) {
 
   query_relative_time_ = control_conf->query_relative_time();
 
+  minimum_speed_protection_ = control_conf->minimum_speed_protection();
+
   return true;
 }
 
@@ -436,8 +438,8 @@ void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug) {
 }
 
 void LatController::UpdateMatrix() {
-  const double v =
-      std::max(VehicleStateProvider::instance()->linear_velocity(), 0.2);
+  const double v = std::max(VehicleStateProvider::instance()->linear_velocity(),
+                            minimum_speed_protection_);
   matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
   matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
   matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;

modules/control/controller/lat_controller.h

@@ -231,6 +231,8 @@ class LatController : public Controller {
   double query_relative_time_;
 
   double pre_steer_angle_ = 0.0;
+
+  double minimum_speed_protection_ = 0.1;
 };
 
 }  // namespace control

modules/control/controller/lon_controller.cc

@@ -18,6 +18,7 @@
 #include <cstdio>
 #include <utility>
 
+#include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/log.h"
 #include "modules/common/math/math_utils.h"
 #include "modules/common/time/time.h"
@@ -85,13 +86,9 @@ void LonController::CloseLogFile() {
     }
   }
 }
-void LonController::Stop() {
-  CloseLogFile();
-}
+void LonController::Stop() { CloseLogFile(); }
 
-LonController::~LonController() {
-  CloseLogFile();
-}
+LonController::~LonController() { CloseLogFile(); }
 
 Status LonController::Init(const ControlConf *control_conf) {
   control_conf_ = control_conf;
@@ -107,6 +104,9 @@ Status LonController::Init(const ControlConf *control_conf) {
   station_pid_controller_.Init(lon_controller_conf.station_pid_conf());
   speed_pid_controller_.Init(lon_controller_conf.low_speed_pid_conf());
 
+  vehicle_param_.CopyFrom(
+      common::VehicleConfigHelper::instance()->GetConfig().vehicle_param());
+
   SetDigitalFilterPitchAngle(lon_controller_conf);
 
   LoadControlCalibrationTable(lon_controller_conf);
@@ -229,7 +229,7 @@ Status LonController::ComputeControlCommand(
   if (std::fabs(debug->preview_acceleration_reference()) <=
           FLAGS_max_acceleration_when_stopped &&
       std::fabs(debug->preview_speed_reference()) <=
-          FLAGS_max_abs_speed_when_stopped) {
+          vehicle_param_.max_abs_speed_when_stopped()) {
     acceleration_cmd = lon_controller_conf.standstill_acceleration();
     AINFO << "Stop location reached";
     debug->set_is_full_stop(true);
@@ -285,7 +285,7 @@ Status LonController::ComputeControlCommand(
   cmd->set_brake(brake_cmd);
 
   if (std::fabs(VehicleStateProvider::instance()->linear_velocity()) <=
-          FLAGS_max_abs_speed_when_stopped ||
+          vehicle_param_.max_abs_speed_when_stopped() ||
       chassis->gear_location() == trajectory_message_->gear() ||
       chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
     cmd->set_gear_location(trajectory_message_->gear());
@@ -302,9 +302,7 @@ Status LonController::Reset() {
   return Status::OK();
 }
 
-std::string LonController::Name() const {
-  return name_;
-}
+std::string LonController::Name() const { return name_; }
 
 void LonController::ComputeLongitudinalErrors(
     const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time,

modules/control/controller/lon_controller.h

@@ -27,6 +27,7 @@
 #include <string>
 #include <vector>
 
+#include "modules/common/configs/proto/vehicle_config.pb.h"
 #include "modules/common/filters/digital_filter.h"
 #include "modules/common/filters/digital_filter_coefficients.h"
 #include "modules/common/vehicle_state/vehicle_state_provider.h"
@@ -131,6 +132,9 @@ class LonController : public Controller {
   common::DigitalFilter digital_filter_pitch_angle_;
 
   const ControlConf *control_conf_ = nullptr;
+
+  // vehicle parameter
+  common::VehicleParam vehicle_param_;
 };
 }  // namespace control
 }  // namespace apollo

modules/control/controller/mpc_controller.cc

@@ -76,20 +76,20 @@ bool MPCController::LoadControlConf(const ControlConf *control_conf) {
     AERROR << "[MPCController] control_conf == nullptr";
     return false;
   }
-  const auto &vehicle_param =
+  const auto &vehicle_param_ =
       VehicleConfigHelper::instance()->GetConfig().vehicle_param();
 
   ts_ = control_conf->mpc_controller_conf().ts();
   CHECK_GT(ts_, 0.0) << "[MPCController] Invalid control update interval.";
   cf_ = control_conf->mpc_controller_conf().cf();
   cr_ = control_conf->mpc_controller_conf().cr();
-  wheelbase_ = vehicle_param.wheel_base();
-  steer_transmission_ratio_ = vehicle_param.steer_ratio();
+  wheelbase_ = vehicle_param_.wheel_base();
+  steer_transmission_ratio_ = vehicle_param_.steer_ratio();
   steer_single_direction_max_degree_ =
-      vehicle_param.max_steer_angle() / steer_transmission_ratio_ / 180 * M_PI;
+      vehicle_param_.max_steer_angle() / steer_transmission_ratio_ / 180 * M_PI;
   max_lat_acc_ = control_conf->mpc_controller_conf().max_lateral_acceleration();
-  max_acceleration_ = vehicle_param.max_acceleration();
-  max_deceleration_ = vehicle_param.max_deceleration();
+  max_acceleration_ = vehicle_param_.max_acceleration();
+  max_deceleration_ = vehicle_param_.max_deceleration();
 
   const double mass_fl = control_conf->mpc_controller_conf().mass_fl();
   const double mass_fr = control_conf->mpc_controller_conf().mass_fr();
@@ -108,6 +108,8 @@ bool MPCController::LoadControlConf(const ControlConf *control_conf) {
   throttle_deadzone_ = control_conf->mpc_controller_conf().throttle_deadzone();
   brake_deadzone_ = control_conf->mpc_controller_conf().brake_deadzone();
 
+  minimum_speed_protection_ = control_conf->minimum_speed_protection();
+
   LoadControlCalibrationTable(control_conf->mpc_controller_conf());
   AINFO << "MPC conf loaded";
   return true;
@@ -269,9 +271,7 @@ Status MPCController::ComputeControlCommand(
     const canbus::Chassis *chassis,
     const planning::ADCTrajectory *planning_published_trajectory,
     ControlCommand *cmd) {
-  constexpr float kMinSpeedProtection = 0.1f;
-  VehicleStateProvider::instance()->set_linear_velocity(
-      std::max(chassis->speed_mps(), kMinSpeedProtection));
+  VehicleStateProvider::instance()->set_linear_velocity(chassis->speed_mps());
 
   trajectory_analyzer_ =
       std::move(TrajectoryAnalyzer(planning_published_trajectory));
@@ -389,7 +389,8 @@ Status MPCController::ComputeControlCommand(
   debug->set_is_full_stop(false);
   if (std::fabs(debug->acceleration_reference()) <=
           FLAGS_max_acceleration_when_stopped &&
-      std::fabs(debug->speed_reference()) <= FLAGS_max_abs_speed_when_stopped) {
+      std::fabs(debug->speed_reference()) <=
+          vehicle_param_.max_abs_speed_when_stopped()) {
     acceleration_cmd = standstill_acceleration_;
     AINFO << "Stop location reached";
     debug->set_is_full_stop(true);
@@ -432,7 +433,7 @@ Status MPCController::ComputeControlCommand(
   debug->set_steering_position(chassis->steering_percentage());
 
   if (std::fabs(VehicleStateProvider::instance()->linear_velocity()) <=
-          FLAGS_max_abs_speed_when_stopped ||
+          vehicle_param_.max_abs_speed_when_stopped() ||
       chassis->gear_location() == planning_published_trajectory->gear() ||
       chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
     cmd->set_gear_location(planning_published_trajectory->gear());
@@ -485,7 +486,8 @@ void MPCController::UpdateStateAnalyticalMatching(SimpleMPCDebug *debug) {
 }
 
 void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
-  const double v = VehicleStateProvider::instance()->linear_velocity();
+  const double v = std::max(VehicleStateProvider::instance()->linear_velocity(),
+                            minimum_speed_protection_);
   matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
   matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
   matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;

modules/control/controller/mpc_controller.h

@@ -247,6 +247,8 @@ class MPCController : public Controller {
 
   // MeanFilter lateral_error_filter;
   common::MeanFilter heading_error_filter_;
+
+  double minimum_speed_protection_ = 0.1;
 };
 
 }  // namespace control

modules/control/proto/control_cmd.proto

@@ -37,7 +37,7 @@ message ControlCommand {
   // parking brake engage. true: engaged
   optional bool parking_brake = 8;
 
-  // target speed, in km/h
+  // target speed, in m/s
   optional double speed = 9;
 
   // target acceleration in m`s^-2

modules/control/proto/control_conf.proto

@@ -39,4 +39,5 @@ message ControlConf {
   optional apollo.control.MPCControllerConf mpc_controller_conf = 16;
 
   optional double query_relative_time = 17;
+  optional double minimum_speed_protection = 18;
 }

modules/control/testdata/conf/lincoln.pb.txt

@@ -10,6 +10,9 @@ soft_estop_brake: 50.0
 active_controllers: LAT_CONTROLLER
 active_controllers: LON_CONTROLLER
 max_steering_percentage_allowed: 100
+minimum_speed_resolution: 0.2
+query_relative_time: 0.8
+minimum_speed_protection: 0.1
 lat_controller_conf {
   ts: 0.01
   preview_window: 0

modules/control/testdata/conf/lincoln_lite.pb.txt

@@ -11,6 +11,8 @@ active_controllers: LAT_CONTROLLER
 active_controllers: LON_CONTROLLER
 max_steering_percentage_allowed: 100
 minimum_speed_resolution: 0.2
+query_relative_time: 0.8
+minimum_speed_protection: 0.1
 lat_controller_conf {
   ts: 0.01
   preview_window: 0