fix: improve control safety

Optimize error handling; when an error occurs, the calculation must be skipped.

Author: daohu527

modules/control/conf/control_conf.pb.txt

@@ -103,9 +103,9 @@ lat_controller_conf {
   }
   reverse_leadlag_conf {
     innerstate_saturation_level: 3000
-    alpha: 1.0
-    beta: 1.0
-    tau: 0.0
+    alpha: 0.7
+    beta: 0.6
+    tau: 0.01
   }
   steer_mrac_conf {
     mrac_model_order: 1

modules/control/control_component.cc

@@ -140,11 +140,12 @@ Status ControlComponent::ProduceControlCommand(
 
   // 2. Safety Pre-Check (Input Validation)
   // Checks timestamps, sensor health, and trajectory integrity.
-  bool critical_failure = safety_manager_->CheckInput(local_view_);
+  SafetyResult input_res = safety_manager_->PreCheck(local_view_);
 
   Status status = Status::OK();
+  bool use_previous_cmd = false;
 
-  if (!critical_failure) {
+  if (!input_res.must_bypass) {
     // 3. Core Control Computation
     // Only run algorithms if system is relatively healthy.
     status = controller_agent_.ComputeControlCommand(
@@ -154,21 +155,32 @@ Status ControlComponent::ProduceControlCommand(
     if (status.ok()) {
       // 4. Safety Post-Check (Output Validation)
       // Sanity check on computed commands (e.g., jerk, steering rate).
-      safety_manager_->CheckOutput(*control_command, previous_cmd_);
+      SafetyResult output_res =
+          safety_manager_->PostCheck(*control_command, previous_cmd_);
+      if (output_res.need_freeze) {
+        use_previous_cmd = true;
+        status = Status(ErrorCode::CONTROL_COMPUTE_ERROR,
+                        "Output Limits Violated (Freeze)");
+      }
     } else {
-      AERROR_EVERY(100) << "Controller Agent Compute failed: "
-                        << status.error_message();
       // Logic failure (e.g., solver error) is treated as an internal fault.
-      controller_agent_.Reset();
+      status = Status(ErrorCode::CONTROL_COMPUTE_ERROR,
+                      "Controller Agent Compute failed");
+      use_previous_cmd = true;
     }
   } else {
-    // skip control algorithm on input failure.
-    // Set status to error, but proceed to Safety Policy to generate Estop cmd.
-    status =
-        Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Safety Input Check Failed");
+    use_previous_cmd = true;
+    status = Status(ErrorCode::CONTROL_COMPUTE_ERROR,
+                    "Input Physics Missing (Bypass)");
   }
 
-  // 5. Apply Safety Policy (The Override)
+  // 5. Freeze Strategy
+  if (use_previous_cmd) {
+    *control_command = previous_cmd_;
+    controller_agent_.Reset();
+  }
+
+  // 6. Apply Safety Policy (The Override)
   // This is the final authority. It overrides the command based on the FSM
   // state (Normal, SoftStop, HardEstop).
   safety_manager_->ApplySafetyPolicy(control_command);
@@ -240,6 +252,10 @@ bool ControlComponent::Proc() {
     // but the Reset command ensures no actuator conflict.
   }
 
+  if (pad_msg != nullptr) {
+    control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
+  }
+
   // 4. Header & Diagnostics
   if (!status.ok()) {
     AERROR_EVERY(100) << "Control Error: " << status.error_message();

modules/control/safety/safety_manager.cc

@@ -35,60 +35,71 @@ bool SafetyManager::Init(const ControlConf& conf) {
     active_faults_.clear();
   }
 
-  trajectory_loss_debouncer_ = std::make_unique<CounterDebouncer>(3);
+  // Control logic runs at 100Hz (10ms/cycle).
+  // Planning logic runs at 10Hz (100ms/cycle).
+  // Setting debouncer to 30 allows for a maximum of 300ms (3 missing planning
+  // frames) before triggering a trajectory loss fault.
+  trajectory_loss_debouncer_ = std::make_unique<CounterDebouncer>(30);
+  output_fault_debouncer_ = std::make_unique<CounterDebouncer>(3);
   return true;
 }
 
-bool SafetyManager::CheckInput(const LocalView& view) {
+SafetyResult SafetyManager::PreCheck(const LocalView& view) {
   std::lock_guard<std::mutex> lk(mutex_);
-  // Clear transient faults from the previous cycle
   active_faults_.clear();
+  SafetyResult result;
 
-  CheckPlanningTrajectory(view);
-  CheckKinematics(view);
+  CheckPlanningTrajectory(view, &result);
 
-  // Determine if state transition is needed based on inputs
-  Arbitrate();
+  CheckKinematics(view, &result);
 
-  // Return true if the system is in a critical state where
-  // running the control algorithm is futile or dangerous.
-  return (current_state_ == SafetyState::kHardEstop ||
-          current_state_ == SafetyState::kFatal);
+  return result;
 }
 
-void SafetyManager::CheckOutput(const ControlCommand& cmd,
-                                const ControlCommand& prev_cmd) {
+SafetyResult SafetyManager::PostCheck(const ControlCommand& cmd,
+                                      const ControlCommand& prev_cmd) {
   std::lock_guard<std::mutex> lk(mutex_);
-  CheckControlOutputDynamic(cmd, prev_cmd);
+  SafetyResult result;
 
-  // Re-arbitrate with output faults included
-  Arbitrate();
+  CheckControlOutputDynamic(cmd, prev_cmd, &result);
+
+  return result;
 }
 
-void SafetyManager::CheckPlanningTrajectory(const LocalView& view) {
-  // Check if trajectory is empty
-  bool is_traj_empty = view.trajectory().trajectory_point().empty();
-  if (trajectory_loss_debouncer_->Update(is_traj_empty)) {
-    ReportFault(CONTROL_FAULT_ID(0x02, 0x03), FaultLevel::LEVEL_SOFT_STOP,
-                FaultSource::SOURCE_INPUT);
+void SafetyManager::CheckPlanningTrajectory(const LocalView& view,
+                                            SafetyResult* result) {
+  bool traj_invalid = false;
+
+  if (view.trajectory().trajectory_point().empty()) {
+    traj_invalid = true;
+  } else {
+    for (const auto& pt : view.trajectory().trajectory_point()) {
+      if (std::isnan(pt.v()) || std::isnan(pt.a()) || std::isinf(pt.v()) ||
+          std::isinf(pt.a())) {
+        traj_invalid = true;
+        break;
+      }
+    }
   }
 
-  if (is_traj_empty) return;
+  if (traj_invalid) {
+    result->must_bypass = true;
+  }
 
-  // Physical Sanity Check (NaN/Inf protection)
-  // This prevents the PID/MPC solvers from crashing or outputting garbage
-  const auto& pt = view.trajectory().trajectory_point(0);
-  if (std::isnan(pt.v()) || std::isnan(pt.a()) || std::isinf(pt.v())) {
-    ReportFault(CONTROL_FAULT_ID(0x02, 0x04), FaultLevel::LEVEL_HARD_ESTOP,
-                FaultSource::SOURCE_INPUT);
+  if (trajectory_loss_debouncer_->Update(traj_invalid)) {
+    ReportFault(0x0203, FaultLevel::LEVEL_SOFT_STOP, FaultSource::SOURCE_INPUT);
   }
 }
 
-void SafetyManager::CheckKinematics(const LocalView& view) {
+void SafetyManager::CheckKinematics(const LocalView& view,
+                                    SafetyResult* result) {
   if (view.trajectory().trajectory_point().empty()) return;
 
   const double kEpsilon = 0.001;
   auto first_pt = view.trajectory().trajectory_point(0);
+
+  // Check if the gear selection conflicts with the intended speed (e.g., a
+  // forward gear is planned for reverse speed).
   if (view.chassis().gear_location() == canbus::Chassis::GEAR_DRIVE &&
       first_pt.v() < -kEpsilon) {
     ReportFault(0x0205, FaultLevel::LEVEL_HARD_ESTOP,
@@ -97,30 +108,21 @@ void SafetyManager::CheckKinematics(const LocalView& view) {
 }
 
 void SafetyManager::CheckControlOutputDynamic(const ControlCommand& cmd,
-                                              const ControlCommand& prev_cmd) {
-  // 1. Steering Rate Protection
-  // Prevents violent steering movements that could destabilize the vehicle
-  double dt = conf_.control_period();
-  if (dt <= 0.0) {
-    dt = 0.01;  // Avoid division by zero
-  }
-
-  double steer_diff =
-      std::abs(cmd.steering_target() - prev_cmd.steering_target());
-  double steer_rate = steer_diff / dt;
-
+                                              const ControlCommand& prev_cmd,
+                                              SafetyResult* result) {
   const auto& vehicle_param =
       common::VehicleConfigHelper::GetConfig().vehicle_param();
 
-  if (steer_rate > vehicle_param.max_steer_angle_rate()) {
-    ReportFault(CONTROL_FAULT_ID(0x03, 0x01), FaultLevel::LEVEL_HARD_ESTOP,
-                FaultSource::SOURCE_OUTPUT);
-    AERROR << "Steer rate limit violation: " << steer_rate;
+  // 1. Physical inspection logic: Acceleration over-limit check
+  bool is_accel_insane =
+      std::abs(cmd.acceleration()) > vehicle_param.max_acceleration();
+
+  if (is_accel_insane) {
+    result->need_freeze = true;  // Intercept this frame
   }
 
-  // 2. Acceleration Sanity Check
-  if (std::abs(cmd.acceleration()) > vehicle_param.max_acceleration()) {
-    ReportFault(CONTROL_FAULT_ID(0x03, 0x02), FaultLevel::LEVEL_HARD_ESTOP,
+  if (output_fault_debouncer_->Update(is_accel_insane)) {
+    ReportFault(0x0302, FaultLevel::LEVEL_HARD_ESTOP,
                 FaultSource::SOURCE_OUTPUT);
   }
 }
@@ -146,6 +148,7 @@ void SafetyManager::Arbitrate() {
 
   // Find the highest severity level among all active faults
   for (const auto& event : active_faults_) {
+    AINFO << event.DebugString();
     if (event.has_level() && event.level() > max_level) {
       max_level = event.level();
     }
@@ -192,6 +195,8 @@ void SafetyManager::ExecuteHardEstop(ControlCommand* cmd) {
 }
 
 void SafetyManager::ApplySafetyPolicy(ControlCommand* cmd) {
+  Arbitrate();
+
   if (current_state_ == SafetyState::kNormal) return;
 
   switch (current_state_) {

modules/control/safety/safety_manager.h

@@ -34,6 +34,11 @@
 namespace apollo {
 namespace control {
 
+struct SafetyResult {
+  bool must_bypass = false;
+  bool need_freeze = false;
+};
+
 class SafetyManager {
  public:
   SafetyManager() = default;
@@ -43,11 +48,12 @@ class SafetyManager {
 
   // Phase 1: Pre-Computation Check
   // Returns true if critical failure detected (Skip Compute)
-  bool CheckInput(const LocalView& view);
+  SafetyResult PreCheck(const LocalView& view);
 
   // Phase 2: Post-Computation Check
   // Validates the calculated command
-  void CheckOutput(const ControlCommand& cmd, const ControlCommand& prev_cmd);
+  SafetyResult PostCheck(const ControlCommand& cmd,
+                         const ControlCommand& prev_cmd);
 
   // Phase 3: Policy Application
   // Overrides the command based on current SafetyState
@@ -59,10 +65,11 @@ class SafetyManager {
   SafetyState GetState() const { return current_state_; }
 
  private:
-  void CheckPlanningTrajectory(const LocalView& view);
-  void CheckKinematics(const LocalView& view);
+  void CheckPlanningTrajectory(const LocalView& view, SafetyResult* result);
+  void CheckKinematics(const LocalView& view, SafetyResult* result);
   void CheckControlOutputDynamic(const ControlCommand& cmd,
-                                 const ControlCommand& prev_cmd);
+                                 const ControlCommand& prev_cmd,
+                                 SafetyResult* result);
   void Arbitrate();
 
   void ExecuteSoftStop(ControlCommand* cmd);
@@ -82,6 +89,7 @@ class SafetyManager {
 
   // Debouncers for signal stability
   std::unique_ptr<CounterDebouncer> trajectory_loss_debouncer_;
+  std::unique_ptr<CounterDebouncer> output_fault_debouncer_;
 };
 
 }  // namespace control