75 lines
2.2 KiB
C
75 lines
2.2 KiB
C
//
|
|
// Created by xemon on 23.11.22.
|
|
//
|
|
|
|
#include "CrashDetection.h"
|
|
#include "CmsisRtosThreadUtils.h"
|
|
#include "SystemDelayInterface.h"
|
|
|
|
#define LOG_SIGN "Опр.Авар."
|
|
#define LOGGER env->logger
|
|
|
|
void CrashDetection_Init(
|
|
tCrashDetection *env,
|
|
tAccelDataFlowInterface *accelData,
|
|
float *asi15threshold,
|
|
tCarFlipDetectionSettings *settings,
|
|
tLoggerInterface *logger
|
|
) {
|
|
env->logger = logger;
|
|
|
|
CarCrashDetection_Init(&env->carCrashDetection, accelData, asi15threshold, settings);
|
|
env->emergencyEvents = CarCrashDetection_GetInterface(&env->carCrashDetection);
|
|
env->simulationInterface = CarCrashDetection_GetSimInterface(&env->carCrashDetection);
|
|
|
|
InitThreadAtrStatic(
|
|
&env->thread.attr,
|
|
"CrashDetection",
|
|
env->thread.controlBlock,
|
|
env->thread.stack,
|
|
osPriorityNormal
|
|
);
|
|
|
|
env->thread.id = 0;
|
|
}
|
|
|
|
|
|
static _Noreturn void CrashDetection_Thread(tCrashDetection *env) {
|
|
uint32_t time = SystemGetMs();
|
|
float asi, maxAsi = 0;
|
|
uint32_t count = 0;
|
|
|
|
for (;;) {
|
|
CarCrashDetection_Processing(&env->carCrashDetection, SystemWaitForever);
|
|
#if false
|
|
asi = EraGlonassAsi15_GetCurrentValue(&env->carCrashDetection.asi15);
|
|
if (maxAsi < asi) {
|
|
maxAsi = asi;
|
|
}
|
|
++count;
|
|
|
|
if (time < SystemGetMs()) {
|
|
LoggerCnInfoStatic(LOGGER, LOG_SIGN, "CCD >")
|
|
|
|
LoggerPrintf(LOGGER, " asi<%f> count %u", maxAsi, count);
|
|
maxAsi = 0;
|
|
count = 0;
|
|
|
|
// LoggerPrintf(LOGGER, "grav[%f %f %f] cos<%f>",
|
|
// env->carCrashDetection.flip.current.value.x,
|
|
// env->carCrashDetection.flip.current.value.y,
|
|
// env->carCrashDetection.flip.current.value.z,
|
|
// env->carCrashDetection.flip.current.angleCos
|
|
// );
|
|
LoggerInfoStatic(LOGGER, LOG_SIGN, "<")
|
|
time = SystemGetMs() + 1000;
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void CrashDetection_StartThread(tCrashDetection *env) {
|
|
if (!env->thread.id) {
|
|
env->thread.id = osThreadNew((osThreadFunc_t) (CrashDetection_Thread), (void *) (env), &env->thread.attr);
|
|
}
|
|
} |