57 lines
1.8 KiB
C
57 lines
1.8 KiB
C
//
|
|
// Created by xemon on 22.11.22.
|
|
//
|
|
#include <SystemDelayInterface.h>
|
|
#include "CarFlipDetection.h"
|
|
|
|
#define NUM_0_8_SQR 0.64
|
|
#define NUM_1_2_SQR 1.44
|
|
#define FLIP_ANTI_FLAY_DELAY_TIME_MS 3000
|
|
#define FLIP_MINIMAL_ACCEL_SQR_LENGTH NUM_0_8_SQR
|
|
#define FLIP_MAXIMAL_ACCEL_SQR_LENGTH NUM_1_2_SQR
|
|
|
|
|
|
void CarFlipDetection_InitSettings(tCarFlipDetectionSettings *settings) {
|
|
settings->enabled = false;
|
|
settings->gravity = (vector3) {0.f, 0.f, 0.f};
|
|
settings->thresholds.antiFlightMs = 3000;
|
|
settings->thresholds.angleCos = -1.f;
|
|
|
|
}
|
|
|
|
void CarFlipDetection_Init(tCarFlipDetection *env, tCarFlipDetectionSettings *settings) {
|
|
env->current.durationMs = 0;
|
|
env->flipDetected = false;
|
|
env->settings = settings;
|
|
}
|
|
|
|
bool CarFlipDetection_IsAccelLenSqrInBounds(float accelSqr) {
|
|
return (accelSqr > FLIP_MINIMAL_ACCEL_SQR_LENGTH) && (accelSqr < FLIP_MAXIMAL_ACCEL_SQR_LENGTH);
|
|
}
|
|
|
|
void CarFlipDetection_ApplyCurrentAccel(tCarFlipDetection *env, vector3 currentValue) {
|
|
env->current.value = currentValue;
|
|
|
|
if (CarFlipDetection_IsAccelLenSqrInBounds(vector3LenSquare(currentValue))) {
|
|
|
|
env->current.angleCos = vector3AngleCos(env->settings->gravity, currentValue);
|
|
|
|
// LOGF("ANGLE COS %i \n",(int)(angleCos*100))
|
|
|
|
if (!env->settings->enabled) {
|
|
env->flipDetected = false;
|
|
return;
|
|
}
|
|
|
|
if (env->current.angleCos < env->settings->thresholds.angleCos) {
|
|
if (env->current.durationMs == 0) {
|
|
env->current.durationMs = SystemGetMs() + env->settings->thresholds.antiFlightMs;
|
|
} else if (env->current.durationMs < SystemGetMs()) {
|
|
env->flipDetected = true;
|
|
}
|
|
} else {
|
|
env->current.durationMs = 0;
|
|
env->flipDetected = 0;
|
|
}
|
|
}
|
|
} |