87 lines
2.6 KiB
C
87 lines
2.6 KiB
C
|
#include "imd_monitoring.h"
|
||
|
|
|
||
|
|
#include "main.h"
|
||
|
|
|
||
|
|
#define FREQ_TIMER 1000 // Hz
|
||
|
|
|
||
|
|
#define FREQ_TOLERANCE 1 // Hz
|
||
|
|
#define FREQ_NORMAL 10 // Hz
|
||
|
|
#define FREQ_UNDERVOLTAGE 20 // Hz
|
||
|
|
#define FREQ_SST 30 // Hz
|
||
|
|
#define FREQ_DEV_ERROR 40 // Hz
|
||
|
|
#define FREQ_GND_FAULT 50 // Hz
|
||
|
|
|
||
|
|
#define RISO_MIN_DUTY_CYCLE 8 // %
|
||
|
|
#define RISO_MAX 50000 // kOhm
|
||
|
|
|
||
|
|
#define PWM_TIMEOUT 200 // ms
|
||
|
|
|
||
|
|
IMDData imd_data;
|
||
|
|
|
||
|
|
static TIM_HandleTypeDef *htim;
|
||
|
|
|
||
|
|
void imd_init(TIM_HandleTypeDef *handle) {
|
||
|
|
htim = handle;
|
||
|
|
HAL_TIM_IC_Start_IT(htim, TIM_CHANNEL_1);
|
||
|
|
HAL_TIM_IC_Start(htim, TIM_CHANNEL_2);
|
||
|
|
|
||
|
|
imd_data.state = IMD_STATE_UNKNOWN;
|
||
|
|
}
|
||
|
|
|
||
|
|
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *handle) {
|
||
|
|
if (handle != htim || htim->Channel != HAL_TIM_ACTIVE_CHANNEL_1) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
uint32_t period = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
|
||
|
|
if (period == 0) {
|
||
|
|
// First edge, ignore
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
imd_data.last_high = HAL_GetTick();
|
||
|
|
|
||
|
|
imd_data.freq = FREQ_TIMER / period;
|
||
|
|
uint32_t high_time = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
|
||
|
|
imd_data.duty_cycle = (100 * high_time) / period;
|
||
|
|
|
||
|
|
// Check PWM frequency for state determination
|
||
|
|
if (imd_data.freq > FREQ_NORMAL - FREQ_TOLERANCE &&
|
||
|
|
imd_data.freq < FREQ_NORMAL + FREQ_TOLERANCE) {
|
||
|
|
imd_data.state = IMD_STATE_NORMAL;
|
||
|
|
} else if (imd_data.freq > FREQ_UNDERVOLTAGE - FREQ_TOLERANCE &&
|
||
|
|
imd_data.freq < FREQ_UNDERVOLTAGE + FREQ_TOLERANCE) {
|
||
|
|
imd_data.state = IMD_STATE_UNDERVOLTAGE;
|
||
|
|
} else if (imd_data.freq > FREQ_SST - FREQ_TOLERANCE &&
|
||
|
|
imd_data.freq < FREQ_SST + FREQ_TOLERANCE) {
|
||
|
|
imd_data.state = IMD_STATE_SST;
|
||
|
|
} else if (imd_data.freq > FREQ_DEV_ERROR - FREQ_TOLERANCE &&
|
||
|
|
imd_data.freq < FREQ_DEV_ERROR + FREQ_TOLERANCE) {
|
||
|
|
imd_data.state = IMD_STATE_DEV_ERROR;
|
||
|
|
} else if (imd_data.freq > FREQ_GND_FAULT - FREQ_TOLERANCE &&
|
||
|
|
imd_data.freq < FREQ_GND_FAULT + FREQ_TOLERANCE) {
|
||
|
|
imd_data.state = IMD_STATE_GND_FAULT;
|
||
|
|
} else {
|
||
|
|
imd_data.state = IMD_STATE_UNKNOWN;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Calculate R_iso
|
||
|
|
if (imd_data.state == IMD_STATE_NORMAL ||
|
||
|
|
imd_data.state == IMD_STATE_UNDERVOLTAGE) {
|
||
|
|
if (imd_data.duty_cycle < RISO_MIN_DUTY_CYCLE) {
|
||
|
|
imd_data.r_iso = RISO_MAX;
|
||
|
|
} else {
|
||
|
|
imd_data.r_iso = (90 * 1200) / (imd_data.duty_cycle - 5) - 1200;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void imd_update() {
|
||
|
|
imd_data.ok = HAL_GPIO_ReadPin(IMD_OK_GPIO_Port, IMD_OK_Pin);
|
||
|
|
if (HAL_GetTick() - imd_data.last_high > PWM_TIMEOUT) {
|
||
|
|
if (HAL_GPIO_ReadPin(IMD_M_GPIO_Port, IMD_M_Pin) == GPIO_PIN_SET) {
|
||
|
|
imd_data.state = IMD_STATE_SHORTCIRCUIT_SUPPLY;
|
||
|
|
} else {
|
||
|
|
imd_data.state = IMD_STATE_SHORTCIRCUIT_GND;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|