Line data    Source code

       1             : /**
       2             :  * @file actuator_driver.c
       3             :  * @brief Implementierung der EPB-Aktor-Ansteuerung.
       4             :  *
       5             :  * @arch SWA-003
       6             :  * @reqs SWE-006 SWE-013 SWE-014 SWE-015
       7             :  *
       8             :  * ASIL: B.
       9             :  */
      10             : #include "actuator_driver.h"
      11             : 
      12             : typedef struct {
      13             :     ActuatorStatus status;
      14             :     uint16_t       over_ms;  /* Milliseconds above current limit (counts in 1 kHz ISR) */
      15             : } ActuatorCtx;
      16             : 
      17             : static ActuatorCtx s_ctx[ACTUATOR_COUNT];
      18             : 
      19         656 : static bool is_valid_id(ActuatorId id)
      20             : {
      21         656 :     return (id == ACTUATOR_LEFT) || (id == ACTUATOR_RIGHT);
      22             : }
      23             : 
      24          46 : EpbStatus actuator_init(void)
      25             : {
      26         138 :     for (uint8_t i = 0U; i < ACTUATOR_COUNT; ++i) {
      27          92 :         s_ctx[i].status.direction        = ACT_DIR_STOP;
      28          92 :         s_ctx[i].status.pwm_percent      = 0U;
      29          92 :         s_ctx[i].status.current_ma       = 0U;
      30          92 :         s_ctx[i].status.peak_current_ma  = 0U;
      31          92 :         s_ctx[i].status.clamping_force_n = 0U;
      32          92 :         s_ctx[i].status.overcurrent      = false;
      33          92 :         s_ctx[i].status.last_error       = EPB_OK;
      34          92 :         s_ctx[i].over_ms                 = 0U;
      35             :     }
      36          46 :     return EPB_OK;
      37             : }
      38             : 
      39          56 : EpbStatus actuator_apply(ActuatorId id, uint8_t pwm_percent)
      40             : {
      41          56 :     if (!is_valid_id(id)) {
      42           2 :         return EPB_EINVAL;
      43             :     }
      44          54 :     if (pwm_percent > 100U) {
      45           2 :         return EPB_EINVAL;
      46             :     }
      47          52 :     if (s_ctx[id].status.overcurrent) {
      48           2 :         return EPB_EOVERCURRENT;
      49             :     }
      50          50 :     s_ctx[id].status.direction       = ACT_DIR_APPLY;
      51          50 :     s_ctx[id].status.pwm_percent     = pwm_percent;
      52          50 :     s_ctx[id].status.peak_current_ma = 0U;
      53          50 :     return EPB_OK;
      54             : }
      55             : 
      56           6 : EpbStatus actuator_release(ActuatorId id, uint8_t pwm_percent)
      57             : {
      58           6 :     if (!is_valid_id(id)) {
      59           0 :         return EPB_EINVAL;
      60             :     }
      61           6 :     if (pwm_percent > 100U) {
      62           0 :         return EPB_EINVAL;
      63             :     }
      64           6 :     if (s_ctx[id].status.overcurrent) {
      65           0 :         return EPB_EOVERCURRENT;
      66             :     }
      67           6 :     s_ctx[id].status.direction   = ACT_DIR_RELEASE;
      68           6 :     s_ctx[id].status.pwm_percent = pwm_percent;
      69           6 :     return EPB_OK;
      70             : }
      71             : 
      72          30 : EpbStatus actuator_stop(ActuatorId id)
      73             : {
      74          30 :     if (!is_valid_id(id)) {
      75           0 :         return EPB_EINVAL;
      76             :     }
      77          30 :     s_ctx[id].status.direction   = ACT_DIR_STOP;
      78          30 :     s_ctx[id].status.pwm_percent = 0U;
      79          30 :     return EPB_OK;
      80             : }
      81             : 
      82          18 : ActuatorStatus actuator_get_status(ActuatorId id)
      83             : {
      84          18 :     if (!is_valid_id(id)) {
      85           0 :         ActuatorStatus empty = {0};
      86           0 :         empty.last_error = EPB_EINVAL;
      87           0 :         return empty;
      88             :     }
      89          18 :     return s_ctx[id].status;
      90             : }
      91             : 
      92         546 : void actuator_isr_1khz(ActuatorId id, uint16_t current_sample_ma)
      93             : {
      94         546 :     if (!is_valid_id(id)) {
      95           0 :         return;
      96             :     }
      97             : 
      98         546 :     s_ctx[id].status.current_ma = current_sample_ma;
      99         546 :     if (current_sample_ma > s_ctx[id].status.peak_current_ma) {
     100          10 :         s_ctx[id].status.peak_current_ma = current_sample_ma;
     101             :     }
     102             : 
     103             :     /* SWE-014: Overcurrent cutoff at > 8 A for > 100 ms */
     104         546 :     if (current_sample_ma > ACT_OVERCURRENT_LIMIT_MA) {
     105         540 :         if (s_ctx[id].over_ms < UINT16_MAX) {
     106         540 :             ++s_ctx[id].over_ms;
     107             :         }
     108         540 :         if (s_ctx[id].over_ms >= ACT_OVERCURRENT_WINDOW_MS) {
     109          44 :             s_ctx[id].status.direction   = ACT_DIR_STOP;
     110          44 :             s_ctx[id].status.pwm_percent = 0U;
     111          44 :             s_ctx[id].status.overcurrent = true;
     112          44 :             s_ctx[id].status.last_error  = EPB_EOVERCURRENT;
     113             :         }
     114             :     } else {
     115           6 :         s_ctx[id].over_ms = 0U;
     116             :     }
     117             : 
     118             :     /* SWE-015: Estimate clamping force from peak current (only on apply). */
     119         546 :     if (s_ctx[id].status.direction == ACT_DIR_APPLY) {
     120         502 :         const uint32_t force = ((uint32_t)s_ctx[id].status.peak_current_ma
     121         502 :                                  * ACT_FORCE_PER_AMP_N) / 1000U;
     122         502 :         s_ctx[id].status.clamping_force_n =
     123             :             (force > UINT16_MAX) ? UINT16_MAX : (uint16_t)force;
     124             :     }
     125             : }