checkupManager.hpp

Go to the documentation of this file.

#pragma once
 
#include <cstdlib>
 
#include "comm/communicatorSettings.hpp"
#include "debugUtils.hpp"
#include "embedded/digitalSender.hpp"
#include "embedded/digitalSettings.hpp"
#include "model/systemData.hpp"
 
// Also known as Orchestrator
class CheckupManager {
private:
  SystemData *_system_data_;  
  // Metro initialCheckupTimestamp{INITIAL_CHECKUP_STEP_TIMEOUT}; ///< Timer for the initial checkup
  // sequence.
 
public:
  Metro _ebs_sound_timestamp_{EBS_BUZZER_TIMEOUT};  
 
  enum class CheckupState {
    WAIT_FOR_ASMS,
    START_TOGGLING_WATCHDOG,
    WAIT_FOR_WATCHDOG,
    STOP_TOGGLING_WATCHDOG,
    CHECK_WATCHDOG,
    CLOSE_SDC,
    WAIT_FOR_AATS,
    WAIT_FOR_TS,
    TOGGLE_VALVE,
    CHECK_PRESSURE,
    CHECK_TIMESTAMPS,
    CHECKUP_COMPLETE
  };
 
  enum class CheckupError { WAITING_FOR_RESPONSE, ERROR, SUCCESS };
 
  CheckupState checkup_state_{
      CheckupState::WAIT_FOR_ASMS};  
 
  explicit CheckupManager(SystemData *system_data) : _system_data_(system_data) {};
 
  void reset_checkup_state();
 
  [[nodiscard]] bool should_stay_manual_driving() const;
 
  bool should_stay_off(DigitalSender *digital_sender);
 
  CheckupError initial_checkup_sequence(DigitalSender *digital_sender);
 
  [[nodiscard]] bool should_go_ready_from_off() const;
 
  [[nodiscard]] bool should_stay_ready() const;
 
  [[nodiscard]] bool should_enter_emergency(State current_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_->digital_data_.pneumatic_line_pressure_ != 0 ||
      _system_data_->digital_data_.asms_on_) {
    return false;
  }
 
  return true;
}
 
inline bool CheckupManager::should_stay_off(DigitalSender *digital_sender) {
  CheckupError init_sequence_state = initial_checkup_sequence(digital_sender);
 
  if (init_sequence_state != CheckupError::SUCCESS) {
    return true;
  }
  return false;
}
 
inline CheckupManager::CheckupError CheckupManager::initial_checkup_sequence(
    DigitalSender *digital_sender) {
  switch (checkup_state_) {
    case CheckupState::WAIT_FOR_ASMS:
      // ASMS Activated?
      if (_system_data_->digital_data_.asms_on_) {
        checkup_state_ = CheckupState::CLOSE_SDC;
      }
      break;
    case CheckupState::CLOSE_SDC:
      // Close SDC
      DigitalSender::close_sdc();
      checkup_state_ = CheckupState::WAIT_FOR_AATS;
 
      break;
    case CheckupState::WAIT_FOR_AATS:
 
      // AATS Activated?
      if (!_system_data_->digital_data_.sdc_open_) {
        // At this point, the emergency signal should be set to false, since it is
        // expected that the RES has already sent all initial emergency signals,
        // and if RES unexpectedly sends another emergency signal, it will be
        // set after the AATS button is pressed.
        _system_data_->failure_detection_.emergency_signal_ = false;
        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::TOGGLE_VALVE;
      }
      break;
    case CheckupState::TOGGLE_VALVE:
      // Toggle EBS Valves
      checkup_state_ = CheckupState::CHECK_PRESSURE;
      DEBUG_PRINT("EBS activated");
      DigitalSender::activate_ebs();
 
      break;
    case CheckupState::CHECK_PRESSURE:
      // Check hydraulic line pressure and pneumatic line pressure
      if (_system_data_->sensors_._hydraulic_line_pressure >= HYDRAULIC_BRAKE_THRESHOLD &&
          _system_data_->digital_data_.pneumatic_line_pressure_) {
        checkup_state_ = CheckupState::CHECK_TIMESTAMPS;
      }
      break;
 
    case CheckupState::CHECK_TIMESTAMPS: {
      // Check if all components have responded and no emergency signal has been sent
      checkup_state_ = CheckupState::CHECK_TIMESTAMPS;
      if (_system_data_->failure_detection_.has_any_component_timed_out() ||
          _system_data_->failure_detection_.emergency_signal_) {
        DEBUG_PRINT("Returning ERROR from CHECK_TIMESTAMPS")
        return CheckupError::ERROR;
      }
      checkup_state_ = CheckupState::CHECKUP_COMPLETE;
      DEBUG_PRINT("Checkup complete and returning success");
      return CheckupError::SUCCESS;
    }
    default:
      break;
  }
  return CheckupError::WAITING_FOR_RESPONSE;
}
 
inline bool CheckupManager::should_go_ready_from_off() const {
  if (!_system_data_->digital_data_.asms_on_ || !_system_data_->failure_detection_.ts_on_ ||
      _system_data_->digital_data_.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_.enter_driving_state();
  return false;
}
 
inline bool CheckupManager::should_enter_emergency(State current_state) const {
  if (current_state == State::AS_READY) {
    return _system_data_->failure_detection_.emergency_signal_ ||
            (_system_data_->digital_data_.pneumatic_line_pressure_ == 0 &&
             _system_data_->r2d_logics_.engageEbsTimestamp
                 .checkWithoutReset()) ||  // 5 seconds have passed since ready state and line
                                           // pressure is 0
           _system_data_->failure_detection_.has_any_component_timed_out() ||
           !_system_data_->digital_data_.asms_on_ || !_system_data_->failure_detection_.ts_on_ ||
            (_system_data_->sensors_._hydraulic_line_pressure < HYDRAULIC_BRAKE_THRESHOLD &&
             _system_data_->r2d_logics_.engageEbsTimestamp.checkWithoutReset()) ||
           _system_data_->digital_data_.sdc_open_;
  } else if (current_state == State::AS_DRIVING) {
    return _system_data_->failure_detection_.has_any_component_timed_out() ||
           _system_data_->failure_detection_.emergency_signal_ ||
           _system_data_->digital_data_.sdc_open_ ||
            (_system_data_->digital_data_.pneumatic_line_pressure_ == 0 &&
             _system_data_->r2d_logics_.releaseEbsTimestamp
                 .checkWithoutReset()) ||  // car has one second to make pneumatic pressure 1
            (_system_data_->sensors_._hydraulic_line_pressure >= HYDRAULIC_BRAKE_THRESHOLD &&
             _system_data_->r2d_logics_.releaseEbsTimestamp
                 .checkWithoutReset()) ||  // car has 1 second to reduce hydraulic pressure
           !_system_data_->digital_data_.asms_on_ ||
           !_system_data_->failure_detection_.ts_on_;
  }
 
  return false;
}
 
inline bool CheckupManager::should_stay_driving() const {
  if (abs(_system_data_->sensors_._left_wheel_rpm) < 0.1 &&
      abs(_system_data_->sensors_._right_wheel_rpm) < 0.1 && _system_data_->mission_finished_) {
    return false;
  }
  return true;
}
 
inline bool CheckupManager::should_stay_mission_finished() const {
  if (_system_data_->digital_data_.asms_on_) {
    return true;
  }
  return false;
}
 
inline bool CheckupManager::emergency_sequence_complete() const {
  if (!_system_data_->digital_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;
}