checkupManager.hpp

Go to the documentation of this file.

#pragma once
 
#include <cstdlib>
 
#include "debugUtils.hpp"
#include "embedded/digitalSender.hpp"
#include "embedded/hardwareSettings.hpp"
#include "model/systemData.hpp"
 
// Also known as Orchestrator
class CheckupManager {
private:
  SystemData *_system_data_;  
  TeensyTimerTool::PeriodicTimer watchdog_timer_;
  Metro _watchdog_toggle_timer_{WATCHDOG_TOGGLE_DURATION};  
  Metro _watchdog_test_timer_{WATCHDOG_TEST_DURATION};      
 
  bool check_pressure_high() const;
 
  bool check_pressure_low() const;
 
  bool failed_to_build_pneumatic_pressure_in_engage_time() const;
  bool failed_to_build_pneumatic_pressure_in_release_time() const;
  bool failed_to_build_hydraulic_pressure_in_time() const;
  bool failed_to_reduce_hydraulic_pressure_in_time() const;
 
  void handle_ebs_check();
 
public:
  Metro _ebs_sound_timestamp_{EBS_BUZZER_TIMEOUT};  
 
  enum class CheckupState {
    WAIT_FOR_ASMS,
    START_TOGGLING_WATCHDOG,
    TOGGLING_WATCHDOG,
    STOP_TOGGLING_WATCHDOG,
    CHECK_WATCHDOG,
    START_TOGGLING_WATCHDOG_AGAIN,
    CHECK_EBS_STORAGE,
    CHECK_BRAKE_PRESSURE,
    CLOSE_SDC,
    WAIT_FOR_ASATS,
    WAIT_FOR_TS,
    EBS_CHECKS, 
    CHECK_TIMESTAMPS,
    CHECKUP_COMPLETE
  };
 
  enum class EbsPressureTestPhase {
    DISABLE_ACTUATOR_1,
    CHECK_ACTUATOR_2,
    ENABLE_ACTUATOR_1,
    DISABLE_ACTUATOR_2,
    CHECK_ACTUATOR_1,
    ENABLE_ACTUATOR_2,
    COMPLETE
  };
 
  enum class CheckupError {
    WAITING_FOR_RESPONSE,
    ERROR_WD_STAYED_READY,
    ERROR_WD_TOGGLE,
    ERROR_TIMESTAMPS_EMERGENCY,
    ERROR,
    SUCCESS
  };
 
  CheckupState checkup_state_{
      CheckupState::WAIT_FOR_ASMS};  
 
  EbsPressureTestPhase pressure_test_phase_{EbsPressureTestPhase::DISABLE_ACTUATOR_1};
 
  explicit CheckupManager(SystemData *system_data) : _system_data_(system_data) {};
 
  void reset_checkup_state();
 
  [[nodiscard]] bool should_stay_manual_driving() const;
 
  bool should_stay_off();
 
  CheckupError initial_checkup_sequence();
 
  [[nodiscard]] bool should_go_ready_from_off() const;
 
  [[nodiscard]] bool should_stay_ready() const;
 
  [[nodiscard]] bool should_enter_emergency(State current_state) const;
 
  bool should_enter_emergency_in_ready_state() const;
  bool should_enter_emergency_in_driving_state() const;
 
  [[nodiscard]] bool should_stay_driving() const;
 
  [[nodiscard]] bool should_stay_mission_finished() const;
 
  [[nodiscard]] bool emergency_sequence_complete() const;
 
  [[nodiscard]] bool res_triggered() const;
 
};
 
inline void CheckupManager::reset_checkup_state() {
  checkup_state_ = CheckupState::WAIT_FOR_ASMS;
  _system_data_->mission_finished_ = false;
}
 
inline bool CheckupManager::should_stay_manual_driving() const {
  if (_system_data_->mission_ != Mission::MANUAL ||
      _system_data_->hardware_data_.pneumatic_line_pressure_ != 0 ||
      _system_data_->hardware_data_.asms_on_) {
    return false;
  }
 
  return true;
}
 
inline bool CheckupManager::should_stay_off() {
  CheckupError init_sequence_state = initial_checkup_sequence();
 
  if (init_sequence_state != CheckupError::SUCCESS) {
    return true;
  }
  return false;
}
 
inline CheckupManager::CheckupError CheckupManager::initial_checkup_sequence() {
  switch (checkup_state_) {
    case CheckupState::WAIT_FOR_ASMS:
      if (_system_data_->hardware_data_.asms_on_) {
        checkup_state_ = CheckupState::START_TOGGLING_WATCHDOG;
        DEBUG_PRINT("ASMS activated, starting watchdog check");
      }
      break;
    case CheckupState::START_TOGGLING_WATCHDOG:
      if (_system_data_->hardware_data_.wd_ready_) {
        checkup_state_ = CheckupState::TOGGLING_WATCHDOG;
        _watchdog_toggle_timer_.reset();
        DEBUG_PRINT("Watchdog ready, starting toggle sequence");
        break;
      }
      DigitalSender::toggle_watchdog();
      break;
 
    case CheckupState::TOGGLING_WATCHDOG:
      // Fail immediately if WD_READY goes low during toggling
      if (!_system_data_->hardware_data_.wd_ready_) {
        DEBUG_PRINT("Watchdog error: WD_READY went low during toggling phase");
        return CheckupError::ERROR_WD_TOGGLE;
      }
 
      DigitalSender::toggle_watchdog();
 
      // Toggle for the specified duration
      if (_watchdog_toggle_timer_.checkWithoutReset()) {
        checkup_state_ = CheckupState::STOP_TOGGLING_WATCHDOG;
        _watchdog_test_timer_.reset();
        DEBUG_PRINT("Watchdog toggle complete, stopping watchdog");
      }
      break;
 
    case CheckupState::STOP_TOGGLING_WATCHDOG:
      _watchdog_test_timer_.reset();
      checkup_state_ = CheckupState::CHECK_WATCHDOG;
      DEBUG_PRINT("Stopping watchdog toggle, beginning verification");
      break;
 
    case CheckupState::CHECK_WATCHDOG:
      if (!_system_data_->hardware_data_.wd_ready_) {
        DigitalSender::close_watchdog_sdc();
 
        checkup_state_ = CheckupState::START_TOGGLING_WATCHDOG_AGAIN;
        DEBUG_PRINT("Watchdog no longer ready - verification successful");
        break;
      }
      if (_watchdog_test_timer_.checkWithoutReset()) {
        DEBUG_PRINT("Watchdog test failed - WD_READY did not go low during verification time");
        return CheckupError::ERROR_WD_STAYED_READY;
      }
      break;
    case CheckupState::START_TOGGLING_WATCHDOG_AGAIN:
      watchdog_timer_.begin([] { DigitalSender::toggle_watchdog(); }, 50'000);
      checkup_state_ = CheckupState::CHECK_EBS_STORAGE;
      break;
    case CheckupState::CHECK_EBS_STORAGE:
      if (_system_data_->hardware_data_.pneumatic_line_pressure_) {
        checkup_state_ = CheckupState::CHECK_BRAKE_PRESSURE;
      }
      break;
    case CheckupState::CHECK_BRAKE_PRESSURE:
      if (_system_data_->hardware_data_._hydraulic_line_pressure >= HYDRAULIC_BRAKE_THRESHOLD) {
        checkup_state_ = CheckupState::WAIT_FOR_ASATS;
      }
      break;
    case CheckupState::WAIT_FOR_ASATS:
 
      if (_system_data_->hardware_data_.asats_pressed_) {
        _system_data_->failure_detection_.emergency_signal_ = false;
        checkup_state_ = CheckupState::CLOSE_SDC;
      }
      break;
    case CheckupState::CLOSE_SDC:
      DigitalSender::close_sdc();
      checkup_state_ = CheckupState::WAIT_FOR_TS;
 
      break;
    case CheckupState::WAIT_FOR_TS:
      if (_system_data_->failure_detection_.ts_on_) {
        DEBUG_PRINT("TS activated");
 
        checkup_state_ = CheckupState::EBS_CHECKS;
      }
      break;
    case CheckupState::EBS_CHECKS:  
      handle_ebs_check();
      break;
    case CheckupState::CHECK_TIMESTAMPS: {
      if (_system_data_->failure_detection_.has_any_component_timed_out() ||
          _system_data_->failure_detection_.emergency_signal_) {
        return CheckupError::ERROR_TIMESTAMPS_EMERGENCY;
      }
      checkup_state_ = CheckupState::CHECKUP_COMPLETE;
      DEBUG_PRINT("Checkup complete and returning success");
      return CheckupError::SUCCESS;
    }
    default:
      break;
  }
  return CheckupError::WAITING_FOR_RESPONSE;
}
 
inline void CheckupManager::handle_ebs_check() {
  switch (pressure_test_phase_) {
    case EbsPressureTestPhase::DISABLE_ACTUATOR_1:
      // Step 10: Disable EBS actuator 1
      DEBUG_PRINT("Disabling EBS actuator 1");
      DigitalSender::disable_ebs_actuator_1();
      pressure_test_phase_ = EbsPressureTestPhase::CHECK_ACTUATOR_2;
      break;
 
    case EbsPressureTestPhase::CHECK_ACTUATOR_2:
      if (check_pressure_high()) {//pressure should be high even with only one actuator
        DEBUG_PRINT("Pressure high confirmed with only actuator 2");
        pressure_test_phase_ = EbsPressureTestPhase::ENABLE_ACTUATOR_1;
      }
      break;
 
    case EbsPressureTestPhase::ENABLE_ACTUATOR_1:
      // Step 12: Enable EBS actuator 1 again
      DEBUG_PRINT("Re-enabling EBS actuator 1");
      DigitalSender::enable_ebs_actuator_1();
      pressure_test_phase_ = EbsPressureTestPhase::DISABLE_ACTUATOR_2;
      break;
 
    case EbsPressureTestPhase::DISABLE_ACTUATOR_2:
      // Step 13: Disable EBS actuator 2
      DEBUG_PRINT("Disabling EBS actuator 2");
      DigitalSender::disable_ebs_actuator_2();
      pressure_test_phase_ = EbsPressureTestPhase::CHECK_ACTUATOR_1;
      break;
 
    case EbsPressureTestPhase::CHECK_ACTUATOR_1:
      // Step 14: Check that the brake pressure is still built up correctly
      if (check_pressure_high()) {
        DEBUG_PRINT("Pressure high confirmed with only actuator 1");
        pressure_test_phase_ = EbsPressureTestPhase::ENABLE_ACTUATOR_2;
      }
      break;
 
    case EbsPressureTestPhase::ENABLE_ACTUATOR_2:
      // Step 15: Enable EBS actuator 2 again
      DEBUG_PRINT("Re-enabling EBS actuator 2");
      DigitalSender::enable_ebs_actuator_2();
      pressure_test_phase_ = EbsPressureTestPhase::COMPLETE;
      break;
 
    case EbsPressureTestPhase::COMPLETE:
      // Step 16: Transition to ready state (isto n fazemos já mas vamos checkar as timestamps e
      // entramos ready dps)
      DEBUG_PRINT("EBS check complete, transitioning to next state");
      pressure_test_phase_ = EbsPressureTestPhase::DISABLE_ACTUATOR_1;
      checkup_state_ = CheckupState::CHECK_TIMESTAMPS;
      break;
  }
}
 
inline bool CheckupManager::should_go_ready_from_off() const {
  if (!_system_data_->hardware_data_.asms_on_ || !_system_data_->failure_detection_.ts_on_ ||
      !_system_data_->hardware_data_.asats_pressed_ ||
      _system_data_->hardware_data_.bspd_sdc_open_) {
    return false;
  }
  _system_data_->r2d_logics_.enter_ready_state();
  return true;
}
 
inline bool CheckupManager::should_stay_ready() const {
  if (!_system_data_->r2d_logics_.r2d) {
    return true;
  }
  _system_data_->r2d_logics_.reset_ebs_timestamp();
  return false;
}
 
// ----------------------------------------------------------
// this is ugly as fuck but I don't care >:(
// ----------------------------------------------------------
inline bool CheckupManager::should_enter_emergency(State current_state) const {
  if (current_state == State::AS_READY) {
    return should_enter_emergency_in_ready_state();
  } else if (current_state == State::AS_DRIVING) {
    return should_enter_emergency_in_driving_state();
  }
  return false;
}
 
bool CheckupManager::should_enter_emergency_in_ready_state() const {
  return _system_data_->failure_detection_.emergency_signal_ ||
         failed_to_build_pneumatic_pressure_in_engage_time() ||
         _system_data_->failure_detection_.has_any_component_timed_out() ||
         !_system_data_->hardware_data_.asms_on_ || !_system_data_->failure_detection_.ts_on_ ||
         failed_to_build_hydraulic_pressure_in_time() ||
         _system_data_->hardware_data_.bspd_sdc_open_;
}
 
bool CheckupManager::should_enter_emergency_in_driving_state() const {
  return _system_data_->failure_detection_.has_any_component_timed_out() ||
         _system_data_->failure_detection_.emergency_signal_ ||
         _system_data_->hardware_data_.bspd_sdc_open_ ||
         failed_to_build_pneumatic_pressure_in_release_time() ||
         failed_to_reduce_hydraulic_pressure_in_time() || !_system_data_->hardware_data_.asms_on_ ||
         !_system_data_->failure_detection_.ts_on_;
}
 
inline bool CheckupManager::check_pressure_high() const {
  return _system_data_->hardware_data_._hydraulic_line_pressure >= HYDRAULIC_BRAKE_THRESHOLD &&
         _system_data_->hardware_data_.pneumatic_line_pressure_;
}
 
inline bool CheckupManager::check_pressure_low() const {
  return _system_data_->hardware_data_._hydraulic_line_pressure < HYDRAULIC_BRAKE_THRESHOLD &&
         _system_data_->hardware_data_.pneumatic_line_pressure_;
}
 
bool CheckupManager::failed_to_build_pneumatic_pressure_in_engage_time() const {
  return _system_data_->hardware_data_.pneumatic_line_pressure_ == 0 &&
         _system_data_->r2d_logics_.engageEbsTimestamp.checkWithoutReset();
}
bool CheckupManager::failed_to_build_pneumatic_pressure_in_release_time() const {
  return _system_data_->hardware_data_.pneumatic_line_pressure_ == 0 &&
         _system_data_->r2d_logics_.releaseEbsTimestamp.checkWithoutReset();
}
bool CheckupManager::failed_to_build_hydraulic_pressure_in_time() const {
  return _system_data_->hardware_data_._hydraulic_line_pressure < HYDRAULIC_BRAKE_THRESHOLD &&
         _system_data_->r2d_logics_.engageEbsTimestamp.checkWithoutReset();
}
 
bool CheckupManager::failed_to_reduce_hydraulic_pressure_in_time() const {
  return _system_data_->hardware_data_._hydraulic_line_pressure >= HYDRAULIC_BRAKE_THRESHOLD &&
         _system_data_->r2d_logics_.releaseEbsTimestamp.checkWithoutReset();
}
 
inline bool CheckupManager::should_stay_driving() const {
  if (abs(_system_data_->hardware_data_._left_wheel_rpm) < 0.1 &&
      abs(_system_data_->hardware_data_._right_wheel_rpm) < 0.1 &&
      _system_data_->mission_finished_) {
    return false;
  }
  return true;
}
 
inline bool CheckupManager::should_stay_mission_finished() const {
  if (_system_data_->hardware_data_.asms_on_) {
    return true;
  }
  return false;
}
 
inline bool CheckupManager::emergency_sequence_complete() const {
  if (!_system_data_->hardware_data_.asms_on_ && _ebs_sound_timestamp_.checkWithoutReset()) {
    return true;
  }
  return false;
}
 
inline bool CheckupManager::res_triggered() const {
  if (_system_data_->failure_detection_.emergency_signal_) {
    return true;
  }
  return false;
}