v0stap/DBW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

base

Browse Source
This commit is contained in:
v0stap
2026-04-05 11:31:20 +02:00
commit 78e6a1b8aa
254 changed files with 175905 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

390
DBW_V2/Core/Src/safety.c Normal file
View File

@@ -0,0 +1,390 @@
/*
* safety.c
*
* Created on: 12 May 2021
* Author: Dmitrijs
*/
#include "main.h"
#include "dbw.h"
#include "ts_comms.h"
#include "safety.h"
const char msg1[42] = { "PPS1 Safety margin >4095 or less then 0\r\n" };
const char msg2[42] = { "PPS2 Safety margin >4095 or less then 0\r\n" };
const char msg3[42] = { "TPS1 Safety margin >4095 or less then 0\r\n" };
const char msg4[42] = { "TPS2 Safety margin >4095 or less then 0\r\n" };
void Flash_Write_Protect(void) {
FLASH_OBProgramInitTypeDef p;
p.OptionType = OPTIONBYTE_WRP;
p.WRPState = OB_WRPSTATE_ENABLE;
HAL_FLASH_Unlock();
HAL_FLASH_OB_Unlock();
HAL_FLASHEx_OBErase();
p.WRPPage = 0x00003FFF;
HAL_FLASHEx_OBProgram(&p);
FLASH_WaitForLastOperation((uint32_t) FLASH_TIMEOUT_VALUE);
HAL_FLASH_OB_Lock();
HAL_FLASH_Lock();
HAL_FLASH_OB_Launch();
}
void Flash_Write_Unprotect(void) {
FLASH_OBProgramInitTypeDef p;
p.OptionType = OPTIONBYTE_WRP;
p.WRPState = OB_WRPSTATE_DISABLE;
HAL_FLASH_Unlock();
HAL_FLASH_OB_Unlock();
HAL_FLASHEx_OBErase();
p.WRPPage = 0x00003FFF;
HAL_FLASHEx_OBProgram(&p);
FLASH_WaitForLastOperation((uint32_t) FLASH_TIMEOUT_VALUE);
HAL_FLASH_OB_Lock();
HAL_FLASH_Lock();
HAL_FLASH_OB_Launch();
}
void Watch_Dog_Update(void) {
IWDG->KR = 0x0000AAAA; // update wd timer
}
void Watch_Dog_Init(void) {
IWDG->KR = 0x0000CCCC; // enable watchdog
IWDG->KR = 0x00005555; // enable register access
while (IWDG->SR)
;
IWDG->PR = 0x00000000; // prescaler LSI / 4
while (IWDG->SR)
;
IWDG->RLR = 1000; // set timeout to 100mS
while (IWDG->SR)
;
IWDG->KR = 0x0000AAAA; // enable register access
while (IWDG->SR)
;
}
void Check_Safety_Limits(void) {
//PPS1
if (config->pps1_min < config->pps1_max) {
if ((config->pps1_margin > config->pps1_min)
|| ((config->pps1_max + config->pps1_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg1, 42);
}
} else {
//config->pps1_min > config->pps1_max
if ((config->pps1_margin > config->pps1_max)
|| ((config->pps1_min + config->pps1_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg1, 42);
}
}
//PPS2
if (config->pps2_min < config->pps2_max) {
if ((config->pps2_margin > config->pps2_min)
|| ((config->pps2_max + config->pps2_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg2, 42);
}
} else {
//config->pps2_min > config->pps1_max
if ((config->pps2_margin > config->pps2_max)
|| ((config->pps2_min + config->pps2_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg2, 42);
}
}
//TPS1
if (config->tps1_min < config->tps1_max) {
if ((config->tps1_margin > config->tps1_min)
|| ((config->tps1_max + config->tps1_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg3, 42);
}
} else {
//config->tps1_min > config->tps1_max
if ((config->tps1_margin > config->tps1_max)
|| ((config->tps1_min + config->tps1_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg3, 42);
}
}
//TPS2
if (config->tps2_min < config->tps2_max) {
if ((config->tps2_margin > config->tps2_min)
|| ((config->tps2_max + config->tps2_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg4, 42);
}
} else {
//config->tps2_min > config->tps1_max
if ((config->tps2_margin > config->tps2_max)
|| ((config->tps2_min + config->tps2_margin) > 4095)) {
//config errror
var.status0 |= DBW_STATUS0_CONF_ERROR_F;
DBW_Stop();
TX_Schedule((unsigned char*) msg4, 42);
}
}
}
void Check_Adc_Range(void) {
//PPS1
if (config->pps1_min < config->pps1_max) {
if (var.pps1_adc < (config->pps1_min - config->pps1_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS1_SS_GND;
}
if (var.pps1_adc > (config->pps1_max + config->pps1_margin)) {
//Fault condition PPS1 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS1_SS_VREF;
}
} else {
//config->pps1_min < config->pps1_max
if (var.pps1_adc < (config->pps1_max - config->pps1_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS1_SS_GND;
}
if (var.pps1_adc > (config->pps1_min + config->pps1_margin)) {
//Fault condition PPS1 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS1_SS_VREF;
}
}
//PPS2
if (config->pps2_min < config->pps2_max) {
if (var.pps2_adc < (config->pps2_min - config->pps2_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS2_SS_GND;
}
if (var.pps2_adc > (config->pps2_max + config->pps2_margin)) {
//Fault condition PPS1 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS1_SS_VREF;
}
} else {
if (var.pps2_adc < (config->pps2_max - config->pps2_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS2_SS_GND;
}
if (var.pps2_adc > (config->pps2_min + config->pps2_margin)) {
//Fault condition PPS1 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS1_SS_VREF;
}
}
//PPS Delta
if ((var.pps_delta > config->pps_delta_margin)
|| (var.pps_delta < ((int16_t) -config->pps_delta_margin))) {
//Fault condition PPS DELTA
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_PPS_DELTA;
}
//##########################################
//TPS1
if (config->tps1_min < config->tps1_max) {
if (var.tps1_adc < (config->tps1_min - config->tps1_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS1_SS_GND;
}
if (var.tps1_adc > (config->tps1_max + config->tps1_margin)) {
//Fault condition TPS1 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS1_SS_VREF;
}
} else {
if (var.tps1_adc < (config->tps1_max - config->tps1_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS1_SS_GND;
}
if (var.tps1_adc > (config->tps1_min + config->tps1_margin)) {
//Fault condition TPS1 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS1_SS_VREF;
}
}
//TPS2
if (config->tps2_min < config->tps2_max) {
if (var.tps2_adc < (config->tps2_min - config->tps2_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS2_SS_GND;
}
if (var.tps2_adc > (config->tps2_max + config->tps2_margin)) {
//Fault condition TPS2 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS2_SS_VREF;
}
} else {
if (var.tps2_adc < (config->tps2_max - config->tps2_margin)) {
//Fault condition PPS1 min
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS2_SS_GND;
}
if (var.tps2_adc > (config->tps2_min + config->tps2_margin)) {
//Fault condition TPS2 MAX
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS2_SS_VREF;
}
}
//TPS Delta
if ((var.tps_delta > config->tps_delta_margin)
|| (var.tps_delta < ((int16_t) -config->tps_delta_margin))) {
//Fault condition PPS DELTA
var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
DBW_Stop();
var.status3 = STATUS3_TPS_DELTA;
}
}
volatile int dbw_target_tmr1, dbw_target_tmr2;
void Safety_TPS_Safety_Timer(void) {
if (dbw_target_tmr1 > 0)
dbw_target_tmr1--;
if (dbw_target_tmr2 > 0)
dbw_target_tmr2--;
}
void Safety_TPS_Safety_Timer_Start(void) {
//enable timers. add 1S time to start, because safety system starts in 1S after startup
dbw_target_tmr1 = config->tps_error_time1 + 1000;
dbw_target_tmr2 = config->tps_error_time2 + 1000;
}
void Check_TPS_Target() {
if (var.tps_error > 0) {
//check if tps is within specified target
if (var.tps_error < config->tps_error_margin1)
dbw_target_tmr1 = config->tps_error_time1;
else {
// if not in range in specified time
if (dbw_target_tmr1 == 0) {
//fault
var.status0 |= DBW_STATUS0_FAULT_F;
DBW_Stop();
var.status2 = STATUS3_TARGET1_FAULT;
}
}
if (var.tps_error < config->tps_error_margin2)
dbw_target_tmr2 = config->tps_error_time2;
else {
// if not in range in specified time
if (dbw_target_tmr2 == 0) {
//fault
var.status0 |= DBW_STATUS0_FAULT_F;
DBW_Stop();
var.status2 = STATUS3_TARGET2_FAULT;
}
}
}
else {
//tps_error<0
//check if tps is within specified target
if (var.tps_error > (-config->tps_error_margin1))
dbw_target_tmr1 = config->tps_error_time1;
else {
// if not in range in specified time
if (dbw_target_tmr1 == 0) {
//fault
var.status0 |= DBW_STATUS0_FAULT_F;
DBW_Stop();
var.status2 = STATUS3_TARGET1_FAULT;
}
}
if (var.tps_error > (-config->tps_error_margin2))
dbw_target_tmr2 = config->tps_error_time2;
else {
// if not in range in specified time
if (dbw_target_tmr2 == 0) {
//fault
var.status0 |= DBW_STATUS0_FAULT_F;
DBW_Stop();
var.status2 = STATUS3_TARGET2_FAULT;
}
}
}
}