123 lines
3.9 KiB
C
123 lines
3.9 KiB
C
#include <sched.h>
|
|
/*
|
|
* asi15.c
|
|
*
|
|
* Created on: Aug 25, 2020
|
|
* Author: FICOM-IT LTD
|
|
*/
|
|
|
|
#include "CarCrashDetection.h"
|
|
#include "SystemDelayInterface.h"
|
|
|
|
#define COS_30_DEG 0.86602540
|
|
#define FLIP_DEFAULT_THRESHOLD_ANGLE_COS COS_30_DEG
|
|
|
|
//Use only from CCD task cause input accel concurrency
|
|
void CarCrashDetection_AutoFlipThreshold(tCarCrashDetection *env) {
|
|
int initialSteps = 10;
|
|
int initialStepsLeft = initialSteps;
|
|
|
|
vector3 initialGravity = {0, 0, 0};
|
|
|
|
for (;;) {
|
|
if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, SystemWaitForever)) {
|
|
initialGravity = vector3Add(
|
|
initialGravity,
|
|
env->currentAcceleration
|
|
);
|
|
--initialStepsLeft;
|
|
if (initialStepsLeft <= 0)break;
|
|
}
|
|
}
|
|
|
|
initialGravity = vector3DivNum(initialGravity, (float) initialSteps);
|
|
|
|
// CarFlipDetection_SetThreshold(&env->flip, initialGravity, FLIP_DEFAULT_THRESHOLD_ANGLE_COS);
|
|
}
|
|
|
|
|
|
void CarCrashDetection_Init(
|
|
tCarCrashDetection *env,
|
|
tAccelDataFlowInterface *accelData,
|
|
float *asi15threshold,
|
|
tCarFlipDetectionSettings *settings
|
|
) {
|
|
env->outEvents = osMessageQueueNew(128, sizeof(tUveosEmergencyEvent), NULL);
|
|
env->accelData = accelData;
|
|
EraGlonassAsi15_Init(&env->asi15, asi15threshold);
|
|
CarFlipDetection_Init(&env->flip, settings);
|
|
env->crashEventGenerated = false;
|
|
env->isStarted = false;
|
|
}
|
|
|
|
void CarCrashDetection_Processing(tCarCrashDetection *env, uint32_t timeout) {
|
|
if(env->isStarted == false){
|
|
env->isStarted = true;
|
|
}
|
|
if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, timeout)) {
|
|
EraGlonassAsi15_ApplyCurrentAccel(&env->asi15, env->currentAcceleration);
|
|
CarFlipDetection_ApplyCurrentAccel(&env->flip, env->currentAcceleration);
|
|
env->asi15This = env->asi15.asi15[env->asi15.counter];
|
|
|
|
if (!env->crashEventGenerated && (env->flip.flipDetected || env->asi15.impactDetected)) {
|
|
tUveosEmergencyEvent crash = {
|
|
.impactValue = EraGlonassAsi15_GetCurrentValue(&env->asi15),
|
|
.flipAngleCos = env->flip.current.angleCos,
|
|
.isFlip = env->flip.flipDetected,
|
|
.isImpact = env->asi15.impactDetected,
|
|
};
|
|
|
|
if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
|
|
env->crashEventGenerated = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool CarCrashDetection_GetNext(tCarCrashDetection *env, tUveosEmergencyEvent *event, uint32_t timeout) {
|
|
return osMessageQueueGet(env->outEvents, event, 0, timeout) == osOK;
|
|
}
|
|
|
|
|
|
tUveosEmergencyEventInterface CarCrashDetection_GetInterface(tCarCrashDetection *env) {
|
|
tUveosEmergencyEventInterface result = {
|
|
.env = env,
|
|
.getNext = (UveosEmergencyGetNextCall) CarCrashDetection_GetNext,
|
|
};
|
|
return result;
|
|
}
|
|
|
|
static void CarCrashDetection_SimulateImpact(tCarCrashDetection *env, float asi15) {
|
|
tUveosEmergencyEvent crash = {
|
|
.impactValue = asi15,
|
|
.flipAngleCos = 0,
|
|
.isFlip = false,
|
|
.isImpact = true,
|
|
};
|
|
|
|
if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
|
|
|
|
}
|
|
}
|
|
|
|
static void CarCrashDetection_SimulateFlip(tCarCrashDetection *env, float angle) {
|
|
tUveosEmergencyEvent crash = {
|
|
.impactValue = 0,
|
|
.flipAngleCos = angle,
|
|
.isFlip = true,
|
|
.isImpact = false,
|
|
};
|
|
|
|
if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
|
|
}
|
|
}
|
|
|
|
tUveosEmergencySimulationInterface CarCrashDetection_GetSimInterface(tCarCrashDetection *env) {
|
|
tUveosEmergencySimulationInterface result = {
|
|
.env = env,
|
|
.simulateImpact = (UveosEmergencySimulate) CarCrashDetection_SimulateImpact,
|
|
.simulateFlip= (UveosEmergencySimulate) CarCrashDetection_SimulateFlip,
|
|
};
|
|
return result;
|
|
}
|